main.cpp

00001 // FRONT-MBED-COMP MAIN CODE
00002 // WRITTEN BY SEB HALL ON 07/07/2022
00003 
00004 #include "mbed.h"
00005 
00006 /*FOR TESTING ONLY:*/ Serial pc(USBTX, USBRX);
00007 
00008 DigitalOut led1(LED1);
00009 DigitalOut led2(LED2);
00010 
00011 const PinName CANWrite = p29;
00012 const PinName CANRead = p30;
00013 const int CANFrequency = 500000;
00014 const unsigned int SteeringMessageID = 0x101;
00015 const unsigned int WheelSpeedMessageID = 0x102;
00016 
00017 AnalogIn SteeringPin(p15);
00018 DigitalIn LeftWheelPin(p11);
00019 DigitalIn RightWheelPin(p12);
00020 
00021 CAN CANBus(CANRead, CANWrite, CANFrequency);
00022 
00023 // FOR PROCESSING WHEEL SPEED
00024 
00025 bool LeftState = true;
00026 bool RightState = true;
00027 Timer leftTimer;
00028 Timer rightTimer;
00029 int LeftCount = 0;
00030 int RightCount = 0;
00031 unsigned int LeftInterval = 0;
00032 unsigned int RightInterval = 0;
00033 
00034 // DATA FOR CAN
00035 
00036 float WheelAngle = 0.0f;
00037 double WheelSpeed = 0.0f;
00038 
00039 // TICKER
00040 
00041 Ticker SendTicker;
00042 
00043 union { double formattedData; unsigned char rawData[8]; } CANConversion;
00044 
00045 void loop();
00046 void SendCANMessage(double data, unsigned int id);
00047 void GetSteeringAngle();
00048 void GetWheelStates();
00049 void WheelSpeedLeft();
00050 void WheelSpeedRight();
00051 void GetWheelSpeed();
00052 
00053 void Send() {
00054     SendCANMessage(WheelSpeed, WheelSpeedMessageID);
00055     SendCANMessage(WheelAngle, SteeringMessageID);
00056     
00057 }
00058 
00059 int main() {
00060     
00061     SendTicker.attach(&Send, 0.1);    
00062     
00063     while(1) {
00064         loop();
00065     }
00066 }
00067 
00068 void loop() {
00069     
00070     GetSteeringAngle();
00071     GetWheelStates();
00072    
00073 }
00074 
00075 void SendCANMessage(double data, unsigned int id) {
00076     
00077     CANMessage message;
00078     CANConversion.formattedData = data;
00079     
00080     for (int i = 0; i < 8; i++) {
00081         message.data[i] = CANConversion.rawData[i];
00082     }
00083     
00084     message.id = id;
00085     CANBus.write(message);
00086     
00087 }
00088 
00089 void GetSteeringAngle() {
00090     float steeringRaw = SteeringPin;
00091     steeringRaw -= 0.5f;
00092     steeringRaw *= 180.0f;
00093     WheelAngle = steeringRaw;
00094         
00095 }
00096 
00097 void GetWheelStates() {
00098     
00099     if (LeftState != LeftWheelPin) {
00100         WheelSpeedLeft();
00101         led1 = !led1;
00102     }
00103     
00104     if (RightState != RightWheelPin) {
00105         WheelSpeedRight();
00106         led2 = !led2;
00107     }
00108     
00109     LeftState = LeftWheelPin;
00110     RightState = RightWheelPin;
00111        
00112 }
00113 
00114 void WheelSpeedLeft() {
00115     
00116     if (LeftCount <= 0)
00117     {
00118         leftTimer.start();
00119     }
00120     LeftCount++;
00121     if (LeftCount == 2)
00122     {
00123         leftTimer.stop();
00124         LeftInterval = leftTimer.read_us();
00125         leftTimer.reset();
00126         LeftCount = 0;
00127         GetWheelSpeed();
00128     }
00129 }
00130 
00131 void WheelSpeedRight() {
00132     if (RightCount <= 0) {
00133         rightTimer.start();
00134     }
00135     RightCount++;
00136     if (RightCount == 2) {
00137         rightTimer.stop();
00138         RightInterval = rightTimer.read_us();
00139         rightTimer.reset();
00140         RightCount = 0;
00141         GetWheelSpeed();
00142     }
00143 }
00144 
00145 void GetWheelSpeed() {
00146     
00147     double left_seconds = (double) LeftInterval;
00148     double right_seconds = (double) RightInterval;
00149     
00150     // seconds per tooth
00151     left_seconds /= 1000000;
00152     right_seconds /= 1000000;
00153     
00154     // seconds per rev
00155     left_seconds *= 20;
00156     right_seconds *= 20;
00157     
00158     double speed = 0;
00159 
00160     if (left_seconds != 0 && right_seconds != 0) {
00161         double leftrpm = (60/left_seconds);
00162         double rightrpm = (60/right_seconds);
00163         speed = (leftrpm + rightrpm)/2;
00164     } else if (left_seconds == 0) {
00165         speed = (60/right_seconds);
00166     } else if (right_seconds == 0) {
00167         speed = (60/left_seconds);
00168     }
00169     
00170     WheelSpeed = speed;
00171     
00172     //pc.printf("SPEED: %6.4f \n\r", WheelSpeed);
00173 }
00174