Team Bath Racing Electric / Mbed 2 deprecated FRONT-MBED-COMP

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@0:6dd6ca23e55a, 2022-07-10 (annotated)

Committer:
samh25
Date:
Sun Jul 10 14:16:59 2022 +0000
Revision:
0:6dd6ca23e55a
The FRONT-MBED code. The main loop checks the steering angle (assuming it is linear) and  wheel speed sensors, calculating the time between each two ticks, and calculating an RPM. These are then send from a ticker function over CAN.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
samh25 0:6dd6ca23e55a 1 // FRONT-MBED-COMP MAIN CODE
samh25 0:6dd6ca23e55a 2 // WRITTEN BY SEB HALL ON 07/07/2022
samh25 0:6dd6ca23e55a 3
samh25 0:6dd6ca23e55a 4 #include "mbed.h"
samh25 0:6dd6ca23e55a 5
samh25 0:6dd6ca23e55a 6 /*FOR TESTING ONLY:*/ Serial pc(USBTX, USBRX);
samh25 0:6dd6ca23e55a 7
samh25 0:6dd6ca23e55a 8 DigitalOut led1(LED1);
samh25 0:6dd6ca23e55a 9 DigitalOut led2(LED2);
samh25 0:6dd6ca23e55a 10
samh25 0:6dd6ca23e55a 11 const PinName CANWrite = p29;
samh25 0:6dd6ca23e55a 12 const PinName CANRead = p30;
samh25 0:6dd6ca23e55a 13 const int CANFrequency = 500000;
samh25 0:6dd6ca23e55a 14 const unsigned int SteeringMessageID = 0x101;
samh25 0:6dd6ca23e55a 15 const unsigned int WheelSpeedMessageID = 0x102;
samh25 0:6dd6ca23e55a 16
samh25 0:6dd6ca23e55a 17 AnalogIn SteeringPin(p15);
samh25 0:6dd6ca23e55a 18 DigitalIn LeftWheelPin(p11);
samh25 0:6dd6ca23e55a 19 DigitalIn RightWheelPin(p12);
samh25 0:6dd6ca23e55a 20
samh25 0:6dd6ca23e55a 21 CAN CANBus(CANRead, CANWrite, CANFrequency);
samh25 0:6dd6ca23e55a 22
samh25 0:6dd6ca23e55a 23 // FOR PROCESSING WHEEL SPEED
samh25 0:6dd6ca23e55a 24
samh25 0:6dd6ca23e55a 25 bool LeftState = true;
samh25 0:6dd6ca23e55a 26 bool RightState = true;
samh25 0:6dd6ca23e55a 27 Timer leftTimer;
samh25 0:6dd6ca23e55a 28 Timer rightTimer;
samh25 0:6dd6ca23e55a 29 int LeftCount = 0;
samh25 0:6dd6ca23e55a 30 int RightCount = 0;
samh25 0:6dd6ca23e55a 31 unsigned int LeftInterval = 0;
samh25 0:6dd6ca23e55a 32 unsigned int RightInterval = 0;
samh25 0:6dd6ca23e55a 33
samh25 0:6dd6ca23e55a 34 // DATA FOR CAN
samh25 0:6dd6ca23e55a 35
samh25 0:6dd6ca23e55a 36 float WheelAngle = 0.0f;
samh25 0:6dd6ca23e55a 37 double WheelSpeed = 0.0f;
samh25 0:6dd6ca23e55a 38
samh25 0:6dd6ca23e55a 39 // TICKER
samh25 0:6dd6ca23e55a 40
samh25 0:6dd6ca23e55a 41 Ticker SendTicker;
samh25 0:6dd6ca23e55a 42
samh25 0:6dd6ca23e55a 43 union { double formattedData; unsigned char rawData[8]; } CANConversion;
samh25 0:6dd6ca23e55a 44
samh25 0:6dd6ca23e55a 45 void loop();
samh25 0:6dd6ca23e55a 46 void SendCANMessage(double data, unsigned int id);
samh25 0:6dd6ca23e55a 47 void GetSteeringAngle();
samh25 0:6dd6ca23e55a 48 void GetWheelStates();
samh25 0:6dd6ca23e55a 49 void WheelSpeedLeft();
samh25 0:6dd6ca23e55a 50 void WheelSpeedRight();
samh25 0:6dd6ca23e55a 51 void GetWheelSpeed();
samh25 0:6dd6ca23e55a 52
samh25 0:6dd6ca23e55a 53 void Send() {
samh25 0:6dd6ca23e55a 54 SendCANMessage(WheelSpeed, WheelSpeedMessageID);
samh25 0:6dd6ca23e55a 55 SendCANMessage(WheelAngle, SteeringMessageID);
samh25 0:6dd6ca23e55a 56
samh25 0:6dd6ca23e55a 57 }
samh25 0:6dd6ca23e55a 58
samh25 0:6dd6ca23e55a 59 int main() {
samh25 0:6dd6ca23e55a 60
samh25 0:6dd6ca23e55a 61 SendTicker.attach(&Send, 0.1);
samh25 0:6dd6ca23e55a 62
samh25 0:6dd6ca23e55a 63 while(1) {
samh25 0:6dd6ca23e55a 64 loop();
samh25 0:6dd6ca23e55a 65 }
samh25 0:6dd6ca23e55a 66 }
samh25 0:6dd6ca23e55a 67
samh25 0:6dd6ca23e55a 68 void loop() {
samh25 0:6dd6ca23e55a 69
samh25 0:6dd6ca23e55a 70 GetSteeringAngle();
samh25 0:6dd6ca23e55a 71 GetWheelStates();
samh25 0:6dd6ca23e55a 72
samh25 0:6dd6ca23e55a 73 }
samh25 0:6dd6ca23e55a 74
samh25 0:6dd6ca23e55a 75 void SendCANMessage(double data, unsigned int id) {
samh25 0:6dd6ca23e55a 76
samh25 0:6dd6ca23e55a 77 CANMessage message;
samh25 0:6dd6ca23e55a 78 CANConversion.formattedData = data;
samh25 0:6dd6ca23e55a 79
samh25 0:6dd6ca23e55a 80 for (int i = 0; i < 8; i++) {
samh25 0:6dd6ca23e55a 81 message.data[i] = CANConversion.rawData[i];
samh25 0:6dd6ca23e55a 82 }
samh25 0:6dd6ca23e55a 83
samh25 0:6dd6ca23e55a 84 message.id = id;
samh25 0:6dd6ca23e55a 85 CANBus.write(message);
samh25 0:6dd6ca23e55a 86
samh25 0:6dd6ca23e55a 87 }
samh25 0:6dd6ca23e55a 88
samh25 0:6dd6ca23e55a 89 void GetSteeringAngle() {
samh25 0:6dd6ca23e55a 90 float steeringRaw = SteeringPin;
samh25 0:6dd6ca23e55a 91 steeringRaw -= 0.5f;
samh25 0:6dd6ca23e55a 92 steeringRaw *= 180.0f;
samh25 0:6dd6ca23e55a 93 WheelAngle = steeringRaw;
samh25 0:6dd6ca23e55a 94
samh25 0:6dd6ca23e55a 95 }
samh25 0:6dd6ca23e55a 96
samh25 0:6dd6ca23e55a 97 void GetWheelStates() {
samh25 0:6dd6ca23e55a 98
samh25 0:6dd6ca23e55a 99 if (LeftState != LeftWheelPin) {
samh25 0:6dd6ca23e55a 100 WheelSpeedLeft();
samh25 0:6dd6ca23e55a 101 led1 = !led1;
samh25 0:6dd6ca23e55a 102 }
samh25 0:6dd6ca23e55a 103
samh25 0:6dd6ca23e55a 104 if (RightState != RightWheelPin) {
samh25 0:6dd6ca23e55a 105 WheelSpeedRight();
samh25 0:6dd6ca23e55a 106 led2 = !led2;
samh25 0:6dd6ca23e55a 107 }
samh25 0:6dd6ca23e55a 108
samh25 0:6dd6ca23e55a 109 LeftState = LeftWheelPin;
samh25 0:6dd6ca23e55a 110 RightState = RightWheelPin;
samh25 0:6dd6ca23e55a 111
samh25 0:6dd6ca23e55a 112 }
samh25 0:6dd6ca23e55a 113
samh25 0:6dd6ca23e55a 114 void WheelSpeedLeft() {
samh25 0:6dd6ca23e55a 115
samh25 0:6dd6ca23e55a 116 if (LeftCount <= 0)
samh25 0:6dd6ca23e55a 117 {
samh25 0:6dd6ca23e55a 118 leftTimer.start();
samh25 0:6dd6ca23e55a 119 }
samh25 0:6dd6ca23e55a 120 LeftCount++;
samh25 0:6dd6ca23e55a 121 if (LeftCount == 2)
samh25 0:6dd6ca23e55a 122 {
samh25 0:6dd6ca23e55a 123 leftTimer.stop();
samh25 0:6dd6ca23e55a 124 LeftInterval = leftTimer.read_us();
samh25 0:6dd6ca23e55a 125 leftTimer.reset();
samh25 0:6dd6ca23e55a 126 LeftCount = 0;
samh25 0:6dd6ca23e55a 127 GetWheelSpeed();
samh25 0:6dd6ca23e55a 128 }
samh25 0:6dd6ca23e55a 129 }
samh25 0:6dd6ca23e55a 130
samh25 0:6dd6ca23e55a 131 void WheelSpeedRight() {
samh25 0:6dd6ca23e55a 132 if (RightCount <= 0) {
samh25 0:6dd6ca23e55a 133 rightTimer.start();
samh25 0:6dd6ca23e55a 134 }
samh25 0:6dd6ca23e55a 135 RightCount++;
samh25 0:6dd6ca23e55a 136 if (RightCount == 2) {
samh25 0:6dd6ca23e55a 137 rightTimer.stop();
samh25 0:6dd6ca23e55a 138 RightInterval = rightTimer.read_us();
samh25 0:6dd6ca23e55a 139 rightTimer.reset();
samh25 0:6dd6ca23e55a 140 RightCount = 0;
samh25 0:6dd6ca23e55a 141 GetWheelSpeed();
samh25 0:6dd6ca23e55a 142 }
samh25 0:6dd6ca23e55a 143 }
samh25 0:6dd6ca23e55a 144
samh25 0:6dd6ca23e55a 145 void GetWheelSpeed() {
samh25 0:6dd6ca23e55a 146
samh25 0:6dd6ca23e55a 147 double left_seconds = (double) LeftInterval;
samh25 0:6dd6ca23e55a 148 double right_seconds = (double) RightInterval;
samh25 0:6dd6ca23e55a 149
samh25 0:6dd6ca23e55a 150 // seconds per tooth
samh25 0:6dd6ca23e55a 151 left_seconds /= 1000000;
samh25 0:6dd6ca23e55a 152 right_seconds /= 1000000;
samh25 0:6dd6ca23e55a 153
samh25 0:6dd6ca23e55a 154 // seconds per rev
samh25 0:6dd6ca23e55a 155 left_seconds *= 20;
samh25 0:6dd6ca23e55a 156 right_seconds *= 20;
samh25 0:6dd6ca23e55a 157
samh25 0:6dd6ca23e55a 158 double speed = 0;
samh25 0:6dd6ca23e55a 159
samh25 0:6dd6ca23e55a 160 if (left_seconds != 0 && right_seconds != 0) {
samh25 0:6dd6ca23e55a 161 double leftrpm = (60/left_seconds);
samh25 0:6dd6ca23e55a 162 double rightrpm = (60/right_seconds);
samh25 0:6dd6ca23e55a 163 speed = (leftrpm + rightrpm)/2;
samh25 0:6dd6ca23e55a 164 } else if (left_seconds == 0) {
samh25 0:6dd6ca23e55a 165 speed = (60/right_seconds);
samh25 0:6dd6ca23e55a 166 } else if (right_seconds == 0) {
samh25 0:6dd6ca23e55a 167 speed = (60/left_seconds);
samh25 0:6dd6ca23e55a 168 }
samh25 0:6dd6ca23e55a 169
samh25 0:6dd6ca23e55a 170 WheelSpeed = speed;
samh25 0:6dd6ca23e55a 171
samh25 0:6dd6ca23e55a 172 //pc.printf("SPEED: %6.4f \n\r", WheelSpeed);
samh25 0:6dd6ca23e55a 173 }
samh25 0:6dd6ca23e55a 174