#include <htc.h> #include "pic16f690.h" #include <stdio.h> #include "bitdefs.h" #include "UARTMotorController.h" __CONFIG(CP_OFF & WDTE_OFF & PWRTE_ON & FOSC_HS); static void UpdateUARTTimer(void); static void ReceiveMachine(void); static void UpdateMotors(void); static void UpdateServos(void); static void UpdateDirectionServo(void); static void UpdateFlags(void); static void UpdateTimeouts(void); //These Variables are used to store the transmission Data static unsigned char Byte_Data1 = 0x00; static unsigned char Byte_Data2 = 0x40; static unsigned char Transmit_FrameID = 1; //These Variables deal with the state of the receiving state machine. static ReceiveState_t ReceiveState = Receive_Idle; static unsigned char ReceiveMessageFlag = 0; static unsigned char ReceiveMessageTimer = 0; static unsigned char ReceiveMessageTimerFlag = 0; static unsigned char ReceiveMessage[20]; static unsigned char ReceiveMessageLength = 0; static unsigned char ReceiveMessageIndex = 0; static unsigned char ReceiveMessageChecksum; //This Variable stores the counter that will be incremented every time the interrupt fires //it handles the timing for the servos. static unsigned char ServoTimer = 0; void interrupt ISR(void){ TMR1IF = 0; //reset flags //Make the interrupt fire next time at half the TMR1L rollover rate TMR1L = 128; TMR1H = 255; //increment our timer variable ServoTimer++; //Perform the servo update with the new time. UpdateServos(); } void InitUART(void){ // Initialization //Clear ANSEL registers ANSEL = 0; ANSELH = 0; //TRIS registers for EUSART TRISB5 = 1; //RX input TRISB6 = 1; //Sort of like slave select line TRISB7 = 0; //TX output //EUSART initialization //9600 baud rate: SPBRG = 129; BRGH = 1; //BRGH; //asynch mode: SYNC = 0; //SYNC; SPEN = 1; //SPEN; //enable tx and rx: TXEN = 1; //TXEN; CREN = 1; //CREN; //Timer0 Init T0CS = 0; //use Fosc/4 PSA = 0; //prescale for T0 PS0 = 1; //Set Prescale to 256. PS1 = 1; PS2 = 1; //Timer1 Init T1CKPS0 = 1; //Compbined thes two prescale bits will give us T1CKPS1 = 0; // a prescale of 2 (00 = 1, 01 = 2, 10 = 4, 11 = 8) TMR1CS = 0; //clock select, 0 for internal fosc/4 TMR1ON = 1; //turn the timer on //this timer will run at 24000000/4/8 = 750000Hz //this means it will roll over at 2929 and roll over big at 11.44 Hz ... measured at 9.5 Hz //11.4 hz will probably be ok, at least according to http://home.earthlink.net/~tdickens/68hc11/servo/servo.html //might want to change the prescale to divide by two though to give 44 hz, and 12khz //did it, and got 38Hz //set up inturrupts TMR1IE = 1; PEIE = 1; GIE = 1; //Timer2 Init T2CKPS0 = 0; //set up prescale to 00 = 1X T2CKPS1 = 0; CCP1M0 = 0;//config mode bits CCP1M1 = 0; CCP1M2 = 1; CCP1M3 = 1; DC1B0 = 0; //690 only DC1B1 = 0; //690 only CCPR1L = 127; TMR2ON = 1; //initialize outputs for Servos 0,1,2 TRISC0 = 0; TRISC1 = 0; TRISC2 = 0; //initialize motor driver ports TRISC4 = 0; //Motor Direction Port TRISC5 = 0; //Motor PWM Port TRISC7 = 1; //AN9 input for Voltage sensor TRISC3 = 1; //inputs for manually setting servos TRISC6 = 1; //inputs for manually setting servos //Initialize input for selecting motor driver 1 or 2 TRISA0 = 1; } void main(void){ InitUART(); while(1){ UpdateUART(); UpdateMotors(); UpdateFlags(); UpdateDirectionServo(); } } //#endif static char Motor0Duty = 128; static char Motor0Dir = 0; static char ReceiveTimeout = 0; static char ReceiveTimeoutLimit = 44; static void UpdateMotors(){ //Parse the received message from the controller for motor direction //different settings on pin RA0 tell us which motor we are controlling if (RA0 ==0){ if ((ReadReceiveMessage(0) & 0b00000001)==0){ RC4 = 0; } else{ RC4 = 1; } } else{ if ((ReadReceiveMessage(0) & 0b00000010)==0){ RC4 = 0; } else{ RC4 = 1; } } } //These variables store the positions for each servo in the various states static char Servo0off = 19;//red static char Servo0on = 36;//red static char Servo1off = 36;//green static char Servo1on = 19;//green static char Servo0Duty = 19;//Servo0off; static char Servo1Duty = 36;//Servo1off; static char Servo2Duty = 27; //straight ahead static unsigned char ServoTimeout = 0; static unsigned char ServoTimeoutLimit = 254; static void UpdateServos(){ //this function is called by the inturrupt ever time timer 1 is rolled over if (RA0 ==0){ if (ServoTimeout < ServoTimeoutLimit){ if (ServoTimer < Servo0Duty){ //make sure the bit is high RC0 = 1; } else { //else make sure the bit is low RC0 = 0; } if (ServoTimer < Servo1Duty){ //make sure the bit is high RC1 = 1; } else { //else make sure the bit is low RC1 = 0; } } } else{ if (ServoTimer < Servo2Duty){ //make sure the bit is high RC0 = 1; } else { //else make sure the bit is low RC0 = 0; } } } static char LastServo0 = 0; static char LastServo1 = 0; //This function parses the received message and sets the servos for flag status static void UpdateFlags(void){ //This function is only this complicated so that updates only happen //once so we can reset the timeout when they happen. With good servo control, thats probably useless if (RA0 == 0){ if ((ReadReceiveMessage(0) & 0b00000100)==0){ //real code //if ((ReadReceiveMessage(0) & 0b00000001)==0){ //test code responds to motor direction instead //this means servo zero should be 0 if (LastServo0 == 1){ //last time it was on so we should turn it off Servo0Duty = Servo0off; LastServo0 = 0; ServoTimeout = 0; } } else{ //this means servo zero should be 1 if (LastServo0 == 0){ //last time it was off so we should turn it on Servo0Duty = Servo0on; LastServo0 =1; ServoTimeout = 0; } } if ((ReadReceiveMessage(0) & 0b00001000)==0){ //real code //if ((ReadReceiveMessage(0) & 0b00000010)==0){ //test code responds to motor direction instead //this means servo 1 should be 0 if (LastServo1 == 1){ //last time it was on so we should turn it off Servo1Duty = Servo1off; LastServo1 = 0; ServoTimeout = 0; } } else{ //this means servo 1 should be 1 if (LastServo1 == 0){ //last time it was off so we should turn it on Servo1Duty = Servo1on; LastServo1 =1; ServoTimeout = 0; } } } } static signed int Motor1power = 0; static signed int Motor2power = 0; static signed int Avgpower = 0; static signed int Differentialpower = 0; static signed int Direction = 128; //This code implements a servo that points in the direction of travel of the boat //(intended to point a light for night use), or at least the intended user control, //pretty cool, and worked, but not actually used for outside in sun at the competition static void UpdateDirectionServo(void){ if (RA0 ==1){ //Handle Direction Motor1power = ReadReceiveMessage(1); Motor2power = ReadReceiveMessage(2); if ((ReadReceiveMessage(0) & 0b00000001)==0){ Motor1power = (Motor1power * -1); } if ((ReadReceiveMessage(0) & 0b00000010)==0){ Motor2power = (Motor2power * -1); } Avgpower = (Motor1power + Motor2power)/2; //now that we have made the power include direction -255 to +255 //straight forward or backward should give a straight ahead value of the difference being zero //When we do the subtraction, we get -510 to +510 with forward being 0 Differentialpower = Motor1power - Motor2power; if(Avgpower < 0){ Avgpower = (Avgpower * -1); } if (Avgpower > 32){ Direction = 128 - (23*Differentialpower / Avgpower); } else if((Differentialpower > 64) || (Differentialpower < -64)){ Direction = 128 - (10*Differentialpower); } else{ //if (Differentialpower > 128){ Direction = 128; } if (Direction >255){ Direction = 255; } if (Direction < 0){ Direction = 0; } Direction = ((Direction)/7) + 9; Servo2Duty = Direction; } } //run the things that have to run to receive messages void UpdateUART(void){ UpdateUARTTimer(); ReceiveMachine(); } //update the timer for timeout ability static void UpdateUARTTimer(void){ if (T0IF == 1){ //if timer 0 overflows T0IF = 0; ReceiveMessageTimer++; if (ReceiveMessageTimer >= 10){ ReceiveMessageTimer = 0; ReceiveMessageTimerFlag = 1; } } } //Run the actual receive state machine, same as on the bee pic, just duplicated here static void ReceiveMachine(void){ if (RCIF == 1){ unsigned char ReceiveNewByte = RCREG; switch (ReceiveState){ case Receive_Idle: if (ReceiveNewByte == Start_Motor){ ReceiveMessageFlag = 0; ReceiveState = Receive_Data; ReceiveMessageTimer = 0; ReceiveMessageIndex = 0; } break; case Receive_Data: ReceiveMessage[ReceiveMessageIndex] = ReceiveNewByte; //ReceiveMessageChecksum += ReceiveNewByte; ReceiveMessageIndex++; if (ReceiveMessageIndex >= 3){ //ReceiveMessageLength){ ReceiveState = Receive_Idle; //ReceiveState = Receive_Checksum; ReceiveMessageFlag = 1; //added //UpdateFlags(); UpdateDirectionServo(); ReceiveTimeout = 0; } ReceiveMessageTimer = 0; break; } } else if (ReceiveMessageTimerFlag == 1){ ReceiveState = Receive_Idle; ReceiveMessageTimerFlag = 0; } } //helper function unsigned char CheckReceiveMessageFlag(void){ unsigned char ReturnVal = ReceiveMessageFlag; ReceiveMessageFlag = 0; return ReturnVal; } //Helper function unsigned char ReadReceiveMessage(unsigned char Index){ return ReceiveMessage[Index]; }