Elct Car Team / Mbed 2 deprecated CarLab_v2

- This code combines steering and driving in one ticker - Fault check is in a ticker instead of while loop

Dependencies:   mbed MMA8451Q

main.cpp@3:25c6bf0abc00, 2021-10-25 (annotated)

Committer:
aalawfi
Date:
Mon Oct 25 01:39:55 2021 +0000
Revision:
3:25c6bf0abc00
Parent:
2:c857935f928e
Child:
6:d2bd68ba99c9
- Created bluetooth.h and moved bluetooth related stuff over there. ; - Moved PI controller algorithm to driving.h

Who changed what in which revision?

UserRevisionLine numberNew contents of line
quarren42 0:0a6756c7e3ed 1 #include "mbed.h"
quarren42 1:c324a2849500 2 #include "driving.h"
aalawfi 2:c857935f928e 3 #include "steering_header.h"
aalawfi 2:c857935f928e 4 #include "steering_methods.h"
aalawfi 2:c857935f928e 5 #include "state_control.h"
quarren42 1:c324a2849500 6 #include "MMA8451Q.h"
quarren42 0:0a6756c7e3ed 7 #include <iostream>
aalawfi 3:25c6bf0abc00 8 #include "bluetooth.h"
quarren42 1:c324a2849500 9 #define MMA8451_I2C_ADDRESS (0x1d<<1)
quarren42 1:c324a2849500 10
quarren42 0:0a6756c7e3ed 11 Serial pc(USBTX, USBRX); // tx, rx
aalawfi 3:25c6bf0abc00 12
quarren42 0:0a6756c7e3ed 13
quarren42 1:c324a2849500 14 PinName const SDA = PTE25;
quarren42 1:c324a2849500 15 PinName const SCL = PTE24;
quarren42 1:c324a2849500 16
quarren42 1:c324a2849500 17 MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
quarren42 1:c324a2849500 18 Ticker motorLoop;
quarren42 1:c324a2849500 19 Timer t;
quarren42 1:c324a2849500 20
aalawfi 3:25c6bf0abc00 21 int main() {
aalawfi 2:c857935f928e 22 state_update();
aalawfi 2:c857935f928e 23
aalawfi 2:c857935f928e 24 //Delcare Onboard LED with blue color
aalawfi 2:c857935f928e 25 DigitalOut led_b(LED_BLUE);
aalawfi 2:c857935f928e 26 //Set the period of the servo motor control signal
aalawfi 2:c857935f928e 27 servo_motor_pwm.period(1/SERVO_MOTOR_FREQ);
aalawfi 2:c857935f928e 28 //Center the servo motor
aalawfi 2:c857935f928e 29 servo_motor_pwm.write(CENTER_DUTY_CYCLE);
aalawfi 2:c857935f928e 30
aalawfi 2:c857935f928e 31 //Start the control systm using a Ticker object
aalawfi 2:c857935f928e 32 steering_control_ticker.attach(&steering_control, TI);
aalawfi 2:c857935f928e 33
aalawfi 2:c857935f928e 34
aalawfi 2:c857935f928e 35 // call landmark_counter wjen a landmark is detected
aalawfi 2:c857935f928e 36 left_landmark_sensor.rise(&landmark_counter);
aalawfi 2:c857935f928e 37 right_landmark_sensor.rise(&landmark_counter);
aalawfi 2:c857935f928e 38 // update status when the button is pressed
aalawfi 2:c857935f928e 39 stop_button.rise(&state_update);
aalawfi 2:c857935f928e 40 run_button.rise(&state_update);
aalawfi 2:c857935f928e 41 wait_button.rise(&state_update);
aalawfi 2:c857935f928e 42
aalawfi 2:c857935f928e 43
quarren42 0:0a6756c7e3ed 44 bt.baud(BLUETOOTHBAUDRATE);
quarren42 0:0a6756c7e3ed 45 //Sets the communication rate of the micro-controller to the Bluetooth module.
quarren42 0:0a6756c7e3ed 46 pc.printf("Hello World!\n");
quarren42 1:c324a2849500 47 bt.printf("Hello World!\n");
quarren42 1:c324a2849500 48
quarren42 1:c324a2849500 49 t.start();
quarren42 1:c324a2849500 50 float time = t.read();
quarren42 1:c324a2849500 51
quarren42 1:c324a2849500 52 //bt.attach(&btReceive);
quarren42 1:c324a2849500 53 motorLoop.attach(&PI,TI);
quarren42 1:c324a2849500 54
quarren42 1:c324a2849500 55 motorLeft.period(1/freq);
quarren42 1:c324a2849500 56 motorRight.period(1/freq);
quarren42 1:c324a2849500 57
quarren42 1:c324a2849500 58 //setpointLeft = 0.0; //target speed, 0.0 to 1.0
quarren42 1:c324a2849500 59 //setpointRight = 0.0;
quarren42 1:c324a2849500 60
quarren42 1:c324a2849500 61 setpointLeft = 0.7; //target speed, 0.0 to 1.0
quarren42 1:c324a2849500 62 setpointRight = 0.7;
quarren42 1:c324a2849500 63
quarren42 0:0a6756c7e3ed 64 //prints the string to the Tera-Term Console using the Bluetooth object ‘bt’.
quarren42 0:0a6756c7e3ed 65 while(1) {
aalawfi 2:c857935f928e 66
aalawfi 2:c857935f928e 67 pc.printf("\n\n");
aalawfi 2:c857935f928e 68
aalawfi 2:c857935f928e 69 pc.printf("\r\nSteering enabled? : %d",steering_enabled);
aalawfi 2:c857935f928e 70 pc.printf("\r\nCurrent duty cycle : %f ", current_duty_cycle);
aalawfi 2:c857935f928e 71 pc.printf("\r\nLeft duty cycle : %f ", left_distance_sensor.read());
aalawfi 2:c857935f928e 72 pc.printf("\r\nRight voltage : %f" , right_distance_sensor.read());
aalawfi 2:c857935f928e 73 pc.printf("\r\nCurrent duty cycle : %f ", current_duty_cycle);
aalawfi 2:c857935f928e 74 fault_check();
aalawfi 2:c857935f928e 75 wait(0.5);
aalawfi 2:c857935f928e 76 switch(current_state ){
aalawfi 2:c857935f928e 77 case STOP : pc.printf("\r\nCurrent state is stop"); break;
aalawfi 2:c857935f928e 78 case RUN: pc.printf("\r\nCurrent state is RUN"); break;
aalawfi 2:c857935f928e 79 case WAIT : pc.printf("\r\nCurrent state is WAIT"); break;
aalawfi 2:c857935f928e 80 };
aalawfi 2:c857935f928e 81 switch(fault_type) {
aalawfi 2:c857935f928e 82 case CLEAR : pc.printf("\r\nFault: CLEAR"); break;
aalawfi 2:c857935f928e 83 case OFF_TRACK: pc.printf("\r\nFault: OFF TRACK"); break;
aalawfi 2:c857935f928e 84 case BUMPER : pc.printf("\r\nFault: OBSTECALE"); break;
aalawfi 2:c857935f928e 85 case LOW_VOLTAGE : pc.printf("Fault: LOW VOLTAGE"); break;
aalawfi 2:c857935f928e 86 }
aalawfi 2:c857935f928e 87
quarren42 1:c324a2849500 88 x = abs(acc.getAccX());
quarren42 0:0a6756c7e3ed 89 pot1Voltage = pot1 * 3.3f;
quarren42 1:c324a2849500 90 batteryVoltage = battInput * 3.3 * battDividerScalar;
quarren42 1:c324a2849500 91 avgCellVoltage = batteryVoltage / 3.0;
quarren42 1:c324a2849500 92
quarren42 0:0a6756c7e3ed 93 //dutyCycleLeft = (pot1 * fullBatScalar);
quarren42 0:0a6756c7e3ed 94 //dutyCycleRight = (pot1 * fullBatScalar);
quarren42 0:0a6756c7e3ed 95
quarren42 1:c324a2849500 96 //speedLeft = (speedSensorLeft * speedSensorScalar);
quarren42 1:c324a2849500 97 //speedRight = (speedSensorRight * speedSensorScalar);
quarren42 1:c324a2849500 98
quarren42 1:c324a2849500 99 speedLeftVolt = (speedSensorLeft * 3.3f);
quarren42 1:c324a2849500 100 speedRightVolt = (speedSensorRight * 3.3f);
quarren42 1:c324a2849500 101
quarren42 1:c324a2849500 102 bt.printf("Duty Cycle = %1.2f ", dutyCycleLeft);
quarren42 1:c324a2849500 103 bt.printf("Speed (V) L,R = %1.2f", speedLeftVolt);
quarren42 1:c324a2849500 104 bt.printf(", %1.2f ", speedRightVolt);
quarren42 1:c324a2849500 105
quarren42 1:c324a2849500 106 bt.printf("Error L,R: %5.2f", errorLeft);
quarren42 1:c324a2849500 107 bt.printf(", %5.2f ", errorRight);
quarren42 1:c324a2849500 108
quarren42 1:c324a2849500 109 bt.printf("ErrorArea: %1.2f ", errorAreaLeft);
quarren42 1:c324a2849500 110 bt.printf("Output: %1.2f ", controllerOutputLeft);
quarren42 1:c324a2849500 111
quarren42 1:c324a2849500 112 bt.printf("Batt Voltage: %1.2f \n",avgCellVoltage);
quarren42 1:c324a2849500 113
quarren42 1:c324a2849500 114 //setpointLeft = pot1;
quarren42 1:c324a2849500 115 //setpointRight = pot1;
quarren42 0:0a6756c7e3ed 116
quarren42 0:0a6756c7e3ed 117
quarren42 1:c324a2849500 118 if (t.read() > 5){
quarren42 1:c324a2849500 119 setpointLeft = 0.0;
quarren42 1:c324a2849500 120 setpointRight = 0.0;
quarren42 1:c324a2849500 121 }
quarren42 1:c324a2849500 122
quarren42 1:c324a2849500 123 if (t.read_ms() > 5100){
quarren42 1:c324a2849500 124 setpointLeft = 0.2;
quarren42 1:c324a2849500 125 setpointRight = 0.2;
quarren42 1:c324a2849500 126 }
quarren42 0:0a6756c7e3ed 127
quarren42 1:c324a2849500 128 if (newData){
quarren42 1:c324a2849500 129 newData = false;
quarren42 1:c324a2849500 130 // bt.printf("Got %c %3i\n",newInputChar, newInputInt);
quarren42 1:c324a2849500 131
quarren42 1:c324a2849500 132 if (newInputChar == 'L')
quarren42 1:c324a2849500 133 setpointLeft = (newInputInt / 100.0f);
quarren42 1:c324a2849500 134 if (newInputChar == 'R')
quarren42 1:c324a2849500 135 setpointRight = (newInputInt / 100.0f);
quarren42 1:c324a2849500 136
quarren42 0:0a6756c7e3ed 137
quarren42 1:c324a2849500 138 //wait(0.1);
quarren42 1:c324a2849500 139 }
quarren42 1:c324a2849500 140 }
quarren42 1:c324a2849500 141 }
quarren42 1:c324a2849500 142