Elct Car Team / Mbed 2 deprecated CarLab

first commit

Dependencies:   mbed MMA8451Q

main.cpp@18:831c1e03d83e, 2021-10-26 (annotated)

Committer:
quarren42
Date:
Tue Oct 26 22:56:10 2021 +0000
Revision:
18:831c1e03d83e
Parent:
17:d2c98ebda90b
Child:
19:65fecaa2a387
- Most recent

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;
aalawfi 7:05ea1c004b49 19 Ticker fault_detector;
quarren42 1:c324a2849500 20 Timer t;
aalawfi 9:5320c2dfb913 21 Ticker controlUpdate;
quarren42 1:c324a2849500 22
aalawfi 9:5320c2dfb913 23 void PID (void) {
aalawfi 9:5320c2dfb913 24 steering_control();
aalawfi 9:5320c2dfb913 25 PI();
aalawfi 9:5320c2dfb913 26 };
quarren42 18:831c1e03d83e 27
quarren42 18:831c1e03d83e 28 DigitalOut mm(PTC17);
aalawfi 3:25c6bf0abc00 29 int main() {
quarren42 18:831c1e03d83e 30 mm=1;
aalawfi 2:c857935f928e 31 state_update();
aalawfi 2:c857935f928e 32 //Delcare Onboard LED with blue color
aalawfi 2:c857935f928e 33 DigitalOut led_b(LED_BLUE);
aalawfi 2:c857935f928e 34 //Set the period of the servo motor control signal
aalawfi 2:c857935f928e 35 servo_motor_pwm.period(1/SERVO_MOTOR_FREQ);
quarren42 6:d2bd68ba99c9 36 motorLeft.period(1/freq);
quarren42 6:d2bd68ba99c9 37 motorRight.period(1/freq);
aalawfi 14:eb9c58c0f8cd 38 //controlUpdate.attach(&PID, TI);
aalawfi 2:c857935f928e 39 //Center the servo motor
aalawfi 14:eb9c58c0f8cd 40 servo_motor_pwm.write(CENTER_DUTY_CYCLE);
aalawfi 9:5320c2dfb913 41 //controlUpdate.attach(&PID, TI);
aalawfi 2:c857935f928e 42 //Start the control systm using a Ticker object
aalawfi 14:eb9c58c0f8cd 43 steering_control_ticker.attach(&steering_control, 0.02);
aalawfi 14:eb9c58c0f8cd 44 motorLoop.attach(&PI,TI);
quarren42 18:831c1e03d83e 45 fault_detector.attach(&fault_check, 0.003);
quarren42 6:d2bd68ba99c9 46 //bt.attach(&btReceive);
aalawfi 2:c857935f928e 47
aalawfi 2:c857935f928e 48 // call landmark_counter wjen a landmark is detected
aalawfi 2:c857935f928e 49 left_landmark_sensor.rise(&landmark_counter);
aalawfi 2:c857935f928e 50 right_landmark_sensor.rise(&landmark_counter);
aalawfi 2:c857935f928e 51 // update status when the button is pressed
aalawfi 2:c857935f928e 52 stop_button.rise(&state_update);
aalawfi 2:c857935f928e 53 run_button.rise(&state_update);
aalawfi 2:c857935f928e 54 wait_button.rise(&state_update);
aalawfi 2:c857935f928e 55
quarren42 0:0a6756c7e3ed 56 bt.baud(BLUETOOTHBAUDRATE);
quarren42 0:0a6756c7e3ed 57 //Sets the communication rate of the micro-controller to the Bluetooth module.
quarren42 0:0a6756c7e3ed 58 pc.printf("Hello World!\n");
quarren42 1:c324a2849500 59 bt.printf("Hello World!\n");
quarren42 1:c324a2849500 60
quarren42 17:d2c98ebda90b 61 //t.start();
quarren42 17:d2c98ebda90b 62 //float time = t.read();
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
quarren42 17:d2c98ebda90b 69 /*
aalawfi 9:5320c2dfb913 70 bt.printf("\n\n");
aalawfi 9:5320c2dfb913 71 bt.printf("\n\rMotor enabled ? : %d ", motor_enabled );
aalawfi 9:5320c2dfb913 72 bt.printf("\n\rSteering enabled ? : %d ", steering_enabled );
aalawfi 9:5320c2dfb913 73 bt.printf("\n\rCurrent duty cycle : %f", current_duty_cycle);
aalawfi 13:0091da3021df 74 bt.printf("\n\Brake enabled ? : %d ", are_brakes_activated );
aalawfi 9:5320c2dfb913 75 bt.printf("\n\rCurrent duty cycle? : %f ", current_duty_cycle );
quarren42 17:d2c98ebda90b 76 */
quarren42 6:d2bd68ba99c9 77 //wait(0.5); //commented out the wait bc it slows down the fault_check, and it breaks the analogIn readings for the driving input
quarren42 6:d2bd68ba99c9 78 //the driving input ticker is faster than the analog.read() function, so all analog.read() methods must be in the main loop
quarren42 18:831c1e03d83e 79 switch(current_state){
aalawfi 9:5320c2dfb913 80 case STOP : bt.printf("\r\nCurrent state is stop"); break;
aalawfi 9:5320c2dfb913 81 case RUN: bt.printf("\r\nCurrent state is RUN"); break;
aalawfi 9:5320c2dfb913 82 case WAIT : bt.printf("\r\nCurrent state is WAIT"); break;
aalawfi 2:c857935f928e 83 };
aalawfi 2:c857935f928e 84 switch(fault_type) {
aalawfi 9:5320c2dfb913 85 case CLEAR : bt.printf("\r\nFault: CLEAR"); break;
aalawfi 9:5320c2dfb913 86 case OFF_TRACK: bt.printf("\r\nFault: OFF TRACK"); break;
aalawfi 9:5320c2dfb913 87 case BUMPER : bt.printf("\r\nFault: OBSTACLE"); break;
aalawfi 9:5320c2dfb913 88 case LOW_VOLTAGE : bt.printf("\r\nFault: LOW VOLTAGE"); break;
aalawfi 2:c857935f928e 89 }
aalawfi 2:c857935f928e 90
quarren42 1:c324a2849500 91 x = abs(acc.getAccX());
quarren42 18:831c1e03d83e 92 //pot1Voltage = pot1 * 3.3f;
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 bt.printf("Duty Cycle = %1.2f ", dutyCycleLeft);
quarren42 1:c324a2849500 100 bt.printf("Speed (V) L,R = %1.2f", speedLeftVolt);
quarren42 1:c324a2849500 101 bt.printf(", %1.2f ", speedRightVolt);
quarren42 1:c324a2849500 102
quarren42 1:c324a2849500 103 bt.printf("Error L,R: %5.2f", errorLeft);
quarren42 1:c324a2849500 104 bt.printf(", %5.2f ", errorRight);
quarren42 1:c324a2849500 105
quarren42 1:c324a2849500 106 bt.printf("ErrorArea: %1.2f ", errorAreaLeft);
quarren42 18:831c1e03d83e 107 pc.printf("Output: %1.2f ", controllerOutputLeft);
quarren42 1:c324a2849500 108
quarren42 18:831c1e03d83e 109 pc.printf("Batt Voltage: %1.2f \n",avgCellVoltage);
quarren42 17:d2c98ebda90b 110
quarren42 1:c324a2849500 111 //setpointLeft = pot1;
quarren42 1:c324a2849500 112 //setpointRight = pot1;
quarren42 0:0a6756c7e3ed 113
aalawfi 9:5320c2dfb913 114 /*
quarren42 1:c324a2849500 115 if (t.read() > 5){
quarren42 1:c324a2849500 116 setpointLeft = 0.0;
quarren42 1:c324a2849500 117 setpointRight = 0.0;
quarren42 1:c324a2849500 118 }
quarren42 1:c324a2849500 119
quarren42 1:c324a2849500 120 if (t.read_ms() > 5100){
quarren42 1:c324a2849500 121 setpointLeft = 0.2;
quarren42 1:c324a2849500 122 setpointRight = 0.2;
quarren42 1:c324a2849500 123 }
aalawfi 9:5320c2dfb913 124 */
quarren42 0:0a6756c7e3ed 125
quarren42 1:c324a2849500 126 if (newData){
quarren42 1:c324a2849500 127 newData = false;
quarren42 1:c324a2849500 128 // bt.printf("Got %c %3i\n",newInputChar, newInputInt);
quarren42 1:c324a2849500 129
quarren42 1:c324a2849500 130 if (newInputChar == 'L')
quarren42 1:c324a2849500 131 setpointLeft = (newInputInt / 100.0f);
quarren42 1:c324a2849500 132 if (newInputChar == 'R')
quarren42 1:c324a2849500 133 setpointRight = (newInputInt / 100.0f);
quarren42 1:c324a2849500 134
quarren42 0:0a6756c7e3ed 135
quarren42 1:c324a2849500 136 //wait(0.1);
quarren42 1:c324a2849500 137 }
quarren42 1:c324a2849500 138 }
quarren42 1:c324a2849500 139 }
quarren42 1:c324a2849500 140