/* File: robotics6a.c Version 2 of basic controller routines for Version 6 of the UE controller board Keil C headers are not guarded. If robotics library is used, its header file should be the only included file unless KEIL_C_H is defined first. */ #include "robotics6a.h" #include /* ------------------------------------------------------------------------ DELAY ROUTINE. With count = 1 this routine provides a delay of about 50 msec. ---------------------------------------------------------------------------*/ void Delay(unsigned char count) { unsigned char i,j; while(count > 0) { for(j=0;j<255;j++) for(i=0;i<130;i++); count--; } /* end while */ } /* end Delay */ /* ------------------------------------------------------------------------ DISPLAY ROUTINES The display is driven through the serial port. Row 1 positions indexed 0-15. Row 2 positions indexed 16-31. Currently, all routines write starting at current position Writing character '/' clears the display Must call InitializeDisplay before using the display ---------------------------------------------------------------------------*/ void InitializeDisplay(void) { TMOD = 0x20; //Timer 1 mode 2 TH1 = 0xCC; //1200 baud at 12 MHz with some error TCON = 0x40; //Start the baud clock SCON = 0x40; //Mode 1 transmit PCON = 0; //Clock rate/32 = 2 x(baud rate) TI = 1; //Initialize busy bit to ready WriteString ("/Initialized"); } /* end InitializeDisplay */ /* ------------------------------------------------------------------------ Write string starting at current display position. Wraps if more characters than available to end of display ---------------------------------------------------------------------------*/ void WriteString (unsigned char *str) { unsigned char pos; pos = 0; while(str[pos]) /* C strings are null-terminated */ { if (TI) /* Ready to accept character */ { SBUF = str[pos]; /* Send character */ pos++; /* Increment to next character */ TI = 0; /* TI reset to 1 when character has been sent */ } } } /* end WriteString */ /* ------------------------------------------------------------------------ Write a char at the current display position ---------------------------------------------------------------------------*/ void WriteChar (unsigned char ch) { while(~TI); /* wait for not busy */ SBUF = ch; /* send the character */ TI = 0; /* TI reset to 1 when character has been sent */ } /* end WriteChar */ /* ------------------------------------------------------------------------ Write an integer as two hex characters at the current display position ---------------------------------------------------------------------------*/ void WriteInt (int number) { unsigned char code hexchar[] = "0123456789ABCDEF"; WriteChar (hexchar[(number & 0xf0) >> 4]); WriteChar (hexchar[number & 0x0f]); } /* end WriteInt */ /* ------------------------------------------------------------------------ MOTOR ROUTINES Speed is -255 to 255, however |speed| needs to be > 190 to be effective for most vehicle designs. Direction is based on sign of speed: < 0 for left/reverse = 0 for stop > 0 for right/forward Port bits control direction: 00 - stop (but motor still on, used for dynamic braking) 01 - "left" 10 - "right" 11 - not used Port bits assigned: M11 M12 M21 M22 M31 M32 M41 M42 Chip select for enabling motors in Port 1 of processor: P1.4 - M1 select P1.3 - M2 select P1.6 - M3 select P1.5 - M4 select 0 for off, 1 for on PWM modules: Module 1 - M1 Module 0 - M2 Module 3 - M3 Module 2 - M4 ---------------------------------------------------------------------------*/ // Interrupt 6 is the PCA interrupt. We are using register bank 2 for the // interrupt as a matter of form. This code does not actually use any // registers. void PWMInterrupt(void) interrupt 6 using 2 { // Clear bit 7 - counter overflow flag // bit 6 is the counter run flag and must be left at 1 // bit 5 is not used // Clear all pwm module interrup flags - bits 0 to 4. CCON = CCON & 0x40; //Clear all motor interrupt flags } /* PWMInterrupt */ void InitializeMotors (void) { /* Turn off all motors */ AllStop(); /* Enable PWM */ CMOD = 0x01; // Clock freq/12, enable interrupt on overflow, (all motors) CCON = 0x40; // Enable PCA to run. // IPx and IPHx set the interrupt priority as 1 of 4 levels. Setting bit 6 // in IP and IPH sets the priority level to 3 (highest) IP = 0x40; //Set PCA to high priority. All others are low IPH = 0x40; //Set PCA to high priority. All others are low IE = 0xC0; //Bit 7 is global enable. Bit 6 is PCA enable. } /* InitializeMotors */ sbit M1select = 0x94; /* Port 1 Bit 4 */ sbit M2select = 0x93; /* Port 1 Bit 3 */ sbit M3select = 0x96; /* Port 1 Bit 6 */ sbit M4select = 0x95; /* Port 1 Bit 5 */ void AllStop(void) { M1select = 0; /* motor 1 select off, bit 4 */ M2select = 0; /* motor 2 select off, bit 3 */ M3select = 0; /* motor 3 select off, bit 6 */ M4select = 0; /* motor 4 select off, bit 5 */ motors = 0; XBYTE[MOTOR_IDX] = motors; } /* end AllStop */ void SetMotor(unsigned char motorID, int speed) { unsigned char shift = 2 * (4 - motorID); unsigned char realSpeed = abs(speed); if (motorID > 4 || motorID == 0) /* error in usage, turn everything off */ { WriteString ("/MOTOR NUM ERROR"); AllStop(); return; } /* motor number out of range */ if (realSpeed > 255) /* cap speed number at 255 */ realSpeed = 255; /* set direction */ motors = motors & ~(MOTOROFF << shift); /* Turn motor num off */ if (speed < 0) /* Turn motor num on left/reverse */ motors = motors | (MOTORLEFT << shift); else if (speed > 0) /* Turn motor num on right/forward */ motors = motors | (MOTORRIGHT << shift); /* set PWM control */ if (realSpeed != 0) /* Enable motor pwm module, set speed */ { switch (motorID) { case 1: // Motor 1 uses module 1 M1select = 1; // Send a 1 to port 1 bit 4 CCAPM1 = PWM_ENABLE; // Enable Motor 1 PWM for 8-bit PWM CCAP1H = 255 - realSpeed; // Set speed break; case 2: // Motor 2 uses module 0 M2select = 1; // Send a 1 to port 1 bit 3 CCAPM0 = PWM_ENABLE; // Enable Motor 2 PWM for 8-bit PWM CCAP0H = 255 - realSpeed; // Set speed break; case 3: // Motor 3 uses module 3 M3select = 1; // Send a 1 to port 1 bit 6 CCAPM3 = PWM_ENABLE; // Enable Motor 3 PWM for 8-bit PWM CCAP3H = 255 - realSpeed; // Set speed break; case 4: // Motor 4 uses module 2 M4select = 1; // Send a 1 to port 1 bit 5 CCAPM2 = PWM_ENABLE; // Enable Motor 4 PWM for 8-bit PWM CCAP2H = 255 - realSpeed; // Set speed break; } } /* end enable PWM, set speed */ else /* speed = 0, disable PWM and turn off motor */ { switch (motorID) { case 1: CCAPM1 = 0; // Disable Motor 1 PWM M1select = 0; // Send a 0 to port 1 bit 4 break; case 2: CCAPM0 = 0; // Disable Motor 2 PWM M2select = 0; // Send a 0 to port 1 bit 3 break; case 3: CCAPM3 = 0; // Disable Motor 3 PWM M3select = 0; // Send a 0 to port 1 bit 6 break; case 4: CCAPM2 = 0; // Disable Motor 2 PWM M4select = 0; // Send a 0 to port 1 bit 5 break; } } /* end set speed */ XBYTE[MOTOR_IDX] = motors; /* set direction control */ } /* SetMotor */ /* ------------------------------------------------------------------------ I/O ROUTINES Input ports Indexed 1-8 (Port A), 17-24 (Port B). Corresponding to the outside 8 pins on each side Returns 1 when closed (switch closing causes 0, negate for positive result) ---------------------------------------------------------------------------*/ sbit P1_1 = 0x91; /* Port 1 Bit 1 */ sbit P1_2 = 0x92; /* Port 1 Bit 2 */ void InitializeIO(void) { /* Enable Ports A and B for input, Port C for output */ XBYTE[COM_PORT_IDX] = 0x92; /* P1 = P1 | (BIT1 | BIT2); */ P1_1 = 1; P1_2 = 1; } /* InitializeIO */ /* ------------------------------------------------------------------------- This routine is for reading Bit 1 or Bit 2 of Port 1 These inputs do not require a pull-up resistor, so will work with the standard LEGO bump switches. ----------------------------------------------------------------------------*/ unsigned char ReadP1 (unsigned char num) { if (num == 1) return (!P1_1); /* return Port 1 Bit 1 */ if (num == 2) return (!P1_2); /* return Port 1 Bit 2 */ WriteString ("/PORT 1 READ ERROR"); return 0; } /* ReadP1 */ unsigned char ReadInputPort (unsigned char num) { unsigned char shift; if (num > 24 || ((num > 8) && (num <= 16)) || num == 0) { WriteString ("/INPUT NUM ERROR"); return 0; } /* input port number out of range */ else { if (num <= 8) /* Port A */ { shift = num - 1; return (!(XBYTE[PORT_A_IDX] & (BITMASK << shift))); } else /* Port B */ { shift = num - 17; return (!(XBYTE[PORT_B_IDX] & (BITMASK << shift))); } } /* read input port */ } /* ReadInputPort */ /*------------------------------------------------------------------- Ouput ports Index 9-16 (Port C), Corresponding to the middle 8 pins Low is 0, high is 1 ---------------------------------------------------------------------*/ extern void SetOutputPort (unsigned char num, unsigned char direction) { unsigned char shift = num - 9; if ((num < 9 || num > 16)) WriteString ("/OUTPUT NUM ERROR"); else { if (direction == LOW) outputs = outputs & ~(BITMASK << shift); /* Pull port num low */ else if (direction == HIGH) outputs = outputs | (BITMASK << shift); /* Pull port num high */ else WriteString ("/OUTPUT DIR ERROR"); } /* set approriate bit */ /* Write output ports */ XBYTE[PORT_C_IDX] = outputs; } /* SetOutputPort */ /* ------------------------------------------------------------------------ A TO D CONVERTER ROUTINES ---------------------------------------------------------------------------*/ sbit notDone = 0xB5; /* Port 3 Bit 5 */ unsigned char GetAtoD (unsigned char channel) { /* OR 8 with channel for single ended entry */ XBYTE[ATOD_IDX] = (channel | 8); /* Delay to allow conversion to finish */ /* Interrupt sets P3.5 to 0 when finished */ notDone = 1; while (notDone); /* just waiting for change in state */ /* Get the data and return it */ return XBYTE[ATOD_IDX]; } /* end GetAtoD */ /* ------------------------------------------------------------------------ SOUND ROUTINES The buzzer is on port 1 at pin 0. Set to 0 for on, 1 for off state = 1 turns buzzer on; state = 0 turns buzzer off ---------------------------------------------------------------------------*/ sbit P1_0 = 0x90; /* Port 1 Pin 0 */ void Sound(unsigned char state) { if (state == OFF) /* Turn buzzer off */ { /* Set P1.0 to 1 */ P1_0 = 1; } /* end if */ else /* Turn buzzer on */ { /* Set P1.0 to 0 */ P1_0 = 0; } /* end else */ } /* end Sound */ /* Beep drives buzzer as a beeper at approximately .25 seconds on, */ /* .25 seconds off for duration number of cycles. E.g., Beep(5) */ /* beeps 5 times. */ void Beep(unsigned char duration) { unsigned char j; for(j = 0; j