main.cpp

00001 #include "mbed.h"
00002 #include "PID.h"
00003 
00004 #define PULSE_PERIOD 1
00005 #define NUM_STEPS 800
00006 
00007 const PinName CANWritePin = p29;
00008 const PinName CANReadPin = p30;
00009 
00010 const int CANFrequency = 500000;
00011 const unsigned SteeringMessageID = 0x101;
00012 const unsigned SteeringRequestID = 0x100;
00013 
00014 Serial pc(USBTX, USBRX);
00015 DigitalOut led1(LED1);
00016 DigitalOut led2(LED2);
00017 
00018 PID SteeringController;
00019 
00020 DigitalOut S_DIR(p24);
00021 DigitalOut S_PUL(p23);
00022 
00023 union { double formattedData; char rawData[8]; } CANConversion;
00024 
00025 float steeringLocation = 0.0f;
00026 float steeringRequest = 0.0f;
00027 
00028 int TargetSteps = 0;
00029 
00030 CAN CANBus(CANReadPin, CANWritePin, CANFrequency);
00031 
00032 void AimForTarget() {
00033      if (TargetSteps < -5) {
00034         S_DIR = 0;
00035         S_PUL = 1;
00036         wait_ms(PULSE_PERIOD);
00037         S_PUL = 0;
00038         wait_ms(PULSE_PERIOD);
00039         TargetSteps += 1;
00040         //pc.printf("STEPS: %d \n\r", numSteps);
00041     } else if (TargetSteps > 5){
00042         S_DIR = 1; //CW
00043         S_PUL = 1;
00044         wait_ms(PULSE_PERIOD);
00045         S_PUL = 0;
00046         wait_ms(PULSE_PERIOD);
00047         TargetSteps -= 1;
00048         //pc.printf("STEPS: %d \n\r", numSteps);
00049     }
00050 }
00051 
00052 void UpdateTarget() {
00053     
00054     CANMessage message;
00055     
00056     if (CANBus.read(message)) {
00057         
00058         if (message.id == SteeringMessageID) {
00059             
00060             for (int i = 0; i < 8; i++) {
00061                 CANConversion.rawData[i] = message.data[i];    
00062             }
00063     
00064             steeringLocation = CANConversion.formattedData;
00065             
00066             led1 = !led1;
00067         
00068         } else if (message.id == SteeringRequestID) {
00069             
00070             for (int i = 0; i < 8; i++) {
00071                 CANConversion.rawData[i] = message.data[i];    
00072             }
00073     
00074             steeringRequest = CANConversion.formattedData;
00075             
00076             led2 = !led2;
00077             
00078         }
00079     }
00080     
00081     float error = (steeringRequest-steeringLocation);
00082     
00083     float steps = (error/360)*NUM_STEPS;
00084     
00085     TargetSteps = floor(steps);
00086     
00087     
00088 }
00089 
00090 int main() {
00091     
00092     pc.printf("STARTED new one\n\r");
00093     CANBus.attach(callback(UpdateTarget));
00094     
00095     led1 = !led1;
00096     
00097     while (1) {
00098         AimForTarget();
00099     }
00100     
00101 }