Team Bath Racing Electric / Mbed 2 deprecated AI-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 PID

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

Committer:
samh25
Date:
Sun Jul 10 14:21:44 2022 +0000
Revision:
0:a2b980143234
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
samh25 0:a2b980143234 1 #include "mbed.h"
samh25 0:a2b980143234 2 #include "PID.h"
samh25 0:a2b980143234 3
samh25 0:a2b980143234 4 #define PULSE_PERIOD 1
samh25 0:a2b980143234 5 #define NUM_STEPS 800
samh25 0:a2b980143234 6
samh25 0:a2b980143234 7 const PinName CANWritePin = p29;
samh25 0:a2b980143234 8 const PinName CANReadPin = p30;
samh25 0:a2b980143234 9
samh25 0:a2b980143234 10 const int CANFrequency = 500000;
samh25 0:a2b980143234 11 const unsigned SteeringMessageID = 0x101;
samh25 0:a2b980143234 12 const unsigned SteeringRequestID = 0x100;
samh25 0:a2b980143234 13
samh25 0:a2b980143234 14 Serial pc(USBTX, USBRX);
samh25 0:a2b980143234 15 DigitalOut led1(LED1);
samh25 0:a2b980143234 16 DigitalOut led2(LED2);
samh25 0:a2b980143234 17
samh25 0:a2b980143234 18 PID SteeringController;
samh25 0:a2b980143234 19
samh25 0:a2b980143234 20 DigitalOut S_DIR(p24);
samh25 0:a2b980143234 21 DigitalOut S_PUL(p23);
samh25 0:a2b980143234 22
samh25 0:a2b980143234 23 union { double formattedData; char rawData[8]; } CANConversion;
samh25 0:a2b980143234 24
samh25 0:a2b980143234 25 float steeringLocation = 0.0f;
samh25 0:a2b980143234 26 float steeringRequest = 0.0f;
samh25 0:a2b980143234 27
samh25 0:a2b980143234 28 int TargetSteps = 0;
samh25 0:a2b980143234 29
samh25 0:a2b980143234 30 CAN CANBus(CANReadPin, CANWritePin, CANFrequency);
samh25 0:a2b980143234 31
samh25 0:a2b980143234 32 void AimForTarget() {
samh25 0:a2b980143234 33 if (TargetSteps < -5) {
samh25 0:a2b980143234 34 S_DIR = 0;
samh25 0:a2b980143234 35 S_PUL = 1;
samh25 0:a2b980143234 36 wait_ms(PULSE_PERIOD);
samh25 0:a2b980143234 37 S_PUL = 0;
samh25 0:a2b980143234 38 wait_ms(PULSE_PERIOD);
samh25 0:a2b980143234 39 TargetSteps += 1;
samh25 0:a2b980143234 40 //pc.printf("STEPS: %d \n\r", numSteps);
samh25 0:a2b980143234 41 } else if (TargetSteps > 5){
samh25 0:a2b980143234 42 S_DIR = 1; //CW
samh25 0:a2b980143234 43 S_PUL = 1;
samh25 0:a2b980143234 44 wait_ms(PULSE_PERIOD);
samh25 0:a2b980143234 45 S_PUL = 0;
samh25 0:a2b980143234 46 wait_ms(PULSE_PERIOD);
samh25 0:a2b980143234 47 TargetSteps -= 1;
samh25 0:a2b980143234 48 //pc.printf("STEPS: %d \n\r", numSteps);
samh25 0:a2b980143234 49 }
samh25 0:a2b980143234 50 }
samh25 0:a2b980143234 51
samh25 0:a2b980143234 52 void UpdateTarget() {
samh25 0:a2b980143234 53
samh25 0:a2b980143234 54 CANMessage message;
samh25 0:a2b980143234 55
samh25 0:a2b980143234 56 if (CANBus.read(message)) {
samh25 0:a2b980143234 57
samh25 0:a2b980143234 58 if (message.id == SteeringMessageID) {
samh25 0:a2b980143234 59
samh25 0:a2b980143234 60 for (int i = 0; i < 8; i++) {
samh25 0:a2b980143234 61 CANConversion.rawData[i] = message.data[i];
samh25 0:a2b980143234 62 }
samh25 0:a2b980143234 63
samh25 0:a2b980143234 64 steeringLocation = CANConversion.formattedData;
samh25 0:a2b980143234 65
samh25 0:a2b980143234 66 led1 = !led1;
samh25 0:a2b980143234 67
samh25 0:a2b980143234 68 } else if (message.id == SteeringRequestID) {
samh25 0:a2b980143234 69
samh25 0:a2b980143234 70 for (int i = 0; i < 8; i++) {
samh25 0:a2b980143234 71 CANConversion.rawData[i] = message.data[i];
samh25 0:a2b980143234 72 }
samh25 0:a2b980143234 73
samh25 0:a2b980143234 74 steeringRequest = CANConversion.formattedData;
samh25 0:a2b980143234 75
samh25 0:a2b980143234 76 led2 = !led2;
samh25 0:a2b980143234 77
samh25 0:a2b980143234 78 }
samh25 0:a2b980143234 79 }
samh25 0:a2b980143234 80
samh25 0:a2b980143234 81 float error = (steeringRequest-steeringLocation);
samh25 0:a2b980143234 82
samh25 0:a2b980143234 83 float steps = (error/360)*NUM_STEPS;
samh25 0:a2b980143234 84
samh25 0:a2b980143234 85 TargetSteps = floor(steps);
samh25 0:a2b980143234 86
samh25 0:a2b980143234 87
samh25 0:a2b980143234 88 }
samh25 0:a2b980143234 89
samh25 0:a2b980143234 90 int main() {
samh25 0:a2b980143234 91
samh25 0:a2b980143234 92 pc.printf("STARTED new one\n\r");
samh25 0:a2b980143234 93 CANBus.attach(callback(UpdateTarget));
samh25 0:a2b980143234 94
samh25 0:a2b980143234 95 led1 = !led1;
samh25 0:a2b980143234 96
samh25 0:a2b980143234 97 while (1) {
samh25 0:a2b980143234 98 AimForTarget();
samh25 0:a2b980143234 99 }
samh25 0:a2b980143234 100
samh25 0:a2b980143234 101 }