Refactoring Ironcup 2020
Dependencies: mbed mbed-rtos MotionSensor EthernetInterface
main.cpp@0:8f5db5085df7, 2020-09-21 (annotated)
- Committer:
- starling
- Date:
- Mon Sep 21 21:42:07 2020 +0000
- Revision:
- 0:8f5db5085df7
12 mar 2020
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
starling | 0:8f5db5085df7 | 1 | #include "FXAS21002.h" |
starling | 0:8f5db5085df7 | 2 | #include "FXOS8700Q.h" |
starling | 0:8f5db5085df7 | 3 | #include "mbed.h" |
starling | 0:8f5db5085df7 | 4 | #include "CarPWM.h" |
starling | 0:8f5db5085df7 | 5 | #include "receiver.h" |
starling | 0:8f5db5085df7 | 6 | #include "Motor.hpp" |
starling | 0:8f5db5085df7 | 7 | #include "rtos.h" |
starling | 0:8f5db5085df7 | 8 | |
starling | 0:8f5db5085df7 | 9 | #include "LED.hpp" |
starling | 0:8f5db5085df7 | 10 | #include "PIDController.hpp" |
starling | 0:8f5db5085df7 | 11 | |
starling | 0:8f5db5085df7 | 12 | #define PI 3.141593 |
starling | 0:8f5db5085df7 | 13 | #define Ts 0.02 // Controller period(Seconds) |
starling | 0:8f5db5085df7 | 14 | #define END_THRESH 4 //For magnetometer calibration |
starling | 0:8f5db5085df7 | 15 | #define START_THRESH 10 //For magnetometer calibration |
starling | 0:8f5db5085df7 | 16 | #define MINIMUM_VELOCITY 15 |
starling | 0:8f5db5085df7 | 17 | #define GYRO_PERIOD 15000 //us |
starling | 0:8f5db5085df7 | 18 | |
starling | 0:8f5db5085df7 | 19 | #define MIN -1.5 |
starling | 0:8f5db5085df7 | 20 | #define MAX 1.5 |
starling | 0:8f5db5085df7 | 21 | |
starling | 0:8f5db5085df7 | 22 | //Control Objetcs |
starling | 0:8f5db5085df7 | 23 | PwmOut servo(PTD1); // Servo connected to pin PTD3 |
starling | 0:8f5db5085df7 | 24 | Motor *motor = new Motor(PTC3); |
starling | 0:8f5db5085df7 | 25 | FXOS8700Q_mag mag(PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1); |
starling | 0:8f5db5085df7 | 26 | FXOS8700Q_acc acc(PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1); |
starling | 0:8f5db5085df7 | 27 | FXAS21002 *gyro = new FXAS21002(PTE25, PTE24); |
starling | 0:8f5db5085df7 | 28 | //Leds Objects |
starling | 0:8f5db5085df7 | 29 | |
starling | 0:8f5db5085df7 | 30 | LED *led = new LED(PTD0, PTC4, PTC12, LED_RED, LED_GREEN, LED_BLUE); |
starling | 0:8f5db5085df7 | 31 | DigitalOut main_led(PTB2); |
starling | 0:8f5db5085df7 | 32 | //Protocol Objects |
starling | 0:8f5db5085df7 | 33 | Receiver rcv; |
starling | 0:8f5db5085df7 | 34 | EthernetInterface eth; |
starling | 0:8f5db5085df7 | 35 | |
starling | 0:8f5db5085df7 | 36 | // PID controller parameters and functions |
starling | 0:8f5db5085df7 | 37 | PIDController *pidController = new PIDController(motor, &servo, gyro); |
starling | 0:8f5db5085df7 | 38 | |
starling | 0:8f5db5085df7 | 39 | // Motor and servo variables |
starling | 0:8f5db5085df7 | 40 | float saved_velocity = 0; |
starling | 0:8f5db5085df7 | 41 | bool brake = false; |
starling | 0:8f5db5085df7 | 42 | |
starling | 0:8f5db5085df7 | 43 | // Magnetometer/Gyro variables and functions |
starling | 0:8f5db5085df7 | 44 | float max_x = 0, max_y = 0, min_x = 0, min_y = 0, x, y; |
starling | 0:8f5db5085df7 | 45 | MotionSensorDataUnits mag_data; |
starling | 0:8f5db5085df7 | 46 | MotionSensorDataCounts mag_raw; |
starling | 0:8f5db5085df7 | 47 | float processMagAngle(); |
starling | 0:8f5db5085df7 | 48 | void magCal(); |
starling | 0:8f5db5085df7 | 49 | |
starling | 0:8f5db5085df7 | 50 | // Protocol |
starling | 0:8f5db5085df7 | 51 | void readProtocol(); |
starling | 0:8f5db5085df7 | 52 | |
starling | 0:8f5db5085df7 | 53 | int main() |
starling | 0:8f5db5085df7 | 54 | { |
starling | 0:8f5db5085df7 | 55 | // Initializing sensors |
starling | 0:8f5db5085df7 | 56 | main_led = 1; |
starling | 0:8f5db5085df7 | 57 | acc.enable(); |
starling | 0:8f5db5085df7 | 58 | gyro->gyro_config(MODE_1); |
starling | 0:8f5db5085df7 | 59 | |
starling | 0:8f5db5085df7 | 60 | // Set initial control configurations |
starling | 0:8f5db5085df7 | 61 | motor->setVelocity(0); |
starling | 0:8f5db5085df7 | 62 | |
starling | 0:8f5db5085df7 | 63 | // Protocol initialization |
starling | 0:8f5db5085df7 | 64 | |
starling | 0:8f5db5085df7 | 65 | printf("Initializing Ethernet!\r\n"); |
starling | 0:8f5db5085df7 | 66 | led->set_leds_color_nxp(LED::RED); |
starling | 0:8f5db5085df7 | 67 | led->set_leds_color_main(LED::RED); |
starling | 0:8f5db5085df7 | 68 | |
starling | 0:8f5db5085df7 | 69 | eth.init(RECEIVER_IFACE_ADDR, RECEIVER_NETMASK_ADDR, RECEIVER_GATEWAY_ADDR); |
starling | 0:8f5db5085df7 | 70 | eth.connect(); |
starling | 0:8f5db5085df7 | 71 | printf("Protocol initialized! \r\n"); |
starling | 0:8f5db5085df7 | 72 | gyro->start_measure(GYRO_PERIOD); |
starling | 0:8f5db5085df7 | 73 | led->set_leds_color_nxp(LED::BLUE); |
starling | 0:8f5db5085df7 | 74 | led->set_leds_color_main(LED::BLUE); |
starling | 0:8f5db5085df7 | 75 | |
starling | 0:8f5db5085df7 | 76 | |
starling | 0:8f5db5085df7 | 77 | rcv.set_socket(); |
starling | 0:8f5db5085df7 | 78 | |
starling | 0:8f5db5085df7 | 79 | main_led = 0; |
starling | 0:8f5db5085df7 | 80 | |
starling | 0:8f5db5085df7 | 81 | pidController->PIDAttach(); |
starling | 0:8f5db5085df7 | 82 | |
starling | 0:8f5db5085df7 | 83 | //main loop |
starling | 0:8f5db5085df7 | 84 | while (1) |
starling | 0:8f5db5085df7 | 85 | { |
starling | 0:8f5db5085df7 | 86 | readProtocol(); |
starling | 0:8f5db5085df7 | 87 | } |
starling | 0:8f5db5085df7 | 88 | } |
starling | 0:8f5db5085df7 | 89 | |
starling | 0:8f5db5085df7 | 90 | void readProtocol() |
starling | 0:8f5db5085df7 | 91 | { |
starling | 0:8f5db5085df7 | 92 | if (!rcv.receive()) |
starling | 0:8f5db5085df7 | 93 | return; |
starling | 0:8f5db5085df7 | 94 | char msg = rcv.get_msg(); |
starling | 0:8f5db5085df7 | 95 | |
starling | 0:8f5db5085df7 | 96 | switch (msg) |
starling | 0:8f5db5085df7 | 97 | { |
starling | 0:8f5db5085df7 | 98 | case NONE: |
starling | 0:8f5db5085df7 | 99 | break; |
starling | 0:8f5db5085df7 | 100 | case BRAKE: |
starling | 0:8f5db5085df7 | 101 | float intensity, b_wait; |
starling | 0:8f5db5085df7 | 102 | rcv.get_brake(&intensity, &b_wait); |
starling | 0:8f5db5085df7 | 103 | if (!brake) |
starling | 0:8f5db5085df7 | 104 | { |
starling | 0:8f5db5085df7 | 105 | led->set_leds_color_nxp(LED::YELLOW); |
starling | 0:8f5db5085df7 | 106 | motor->stopJogging(); |
starling | 0:8f5db5085df7 | 107 | setServoPWM(0, servo); |
starling | 0:8f5db5085df7 | 108 | pidController->PIDDetach(); |
starling | 0:8f5db5085df7 | 109 | motor->brakeMotor(intensity, b_wait); |
starling | 0:8f5db5085df7 | 110 | pidController->PIDAttach(); |
starling | 0:8f5db5085df7 | 111 | |
starling | 0:8f5db5085df7 | 112 | saved_velocity = 0; |
starling | 0:8f5db5085df7 | 113 | brake = true; |
starling | 0:8f5db5085df7 | 114 | } |
starling | 0:8f5db5085df7 | 115 | break; |
starling | 0:8f5db5085df7 | 116 | case GYRO_ZERO: |
starling | 0:8f5db5085df7 | 117 | gyro->stop_measure(); |
starling | 0:8f5db5085df7 | 118 | wait(0.05); |
starling | 0:8f5db5085df7 | 119 | gyro->start_measure(GYRO_PERIOD); |
starling | 0:8f5db5085df7 | 120 | break; |
starling | 0:8f5db5085df7 | 121 | case ANG_SET: |
starling | 0:8f5db5085df7 | 122 | led->set_leds_color_nxp(LED::PURPLE); |
starling | 0:8f5db5085df7 | 123 | //printf("ANG_SET\r\n"); |
starling | 0:8f5db5085df7 | 124 | pidController->setGyroReference(gyro->get_angle()); |
starling | 0:8f5db5085df7 | 125 | pidController->initializeController(); |
starling | 0:8f5db5085df7 | 126 | break; |
starling | 0:8f5db5085df7 | 127 | case ANG_RST: |
starling | 0:8f5db5085df7 | 128 | //printf("ANG_RST\r\n"); |
starling | 0:8f5db5085df7 | 129 | pidController->setGyroReference(0); |
starling | 0:8f5db5085df7 | 130 | led->set_leds_color_nxp(LED::PURPLE); |
starling | 0:8f5db5085df7 | 131 | pidController->initializeController(); |
starling | 0:8f5db5085df7 | 132 | break; |
starling | 0:8f5db5085df7 | 133 | case MAG_ANG_REF: |
starling | 0:8f5db5085df7 | 134 | led->set_leds_color_nxp(LED::BLUE); |
starling | 0:8f5db5085df7 | 135 | pidController->setTargetAngle(rcv.get_mag_ang_ref() - processMagAngle()); |
starling | 0:8f5db5085df7 | 136 | break; |
starling | 0:8f5db5085df7 | 137 | case ABS_ANG_REF: |
starling | 0:8f5db5085df7 | 138 | led->set_leds_color_nxp(LED::GREEN); |
starling | 0:8f5db5085df7 | 139 | pidController->setTargetAngle(rcv.get_abs_ang_ref()); |
starling | 0:8f5db5085df7 | 140 | break; |
starling | 0:8f5db5085df7 | 141 | case REL_ANG_REF: |
starling | 0:8f5db5085df7 | 142 | led->set_leds_color_nxp(LED::RED); |
starling | 0:8f5db5085df7 | 143 | pidController->setTargetAngle(rcv.get_rel_ang_ref() + gyro->get_angle() * PI / 180); |
starling | 0:8f5db5085df7 | 144 | break; |
starling | 0:8f5db5085df7 | 145 | case GND_VEL: |
starling | 0:8f5db5085df7 | 146 | led->set_leds_color_nxp(LED::AQUA); |
starling | 0:8f5db5085df7 | 147 | saved_velocity = rcv.get_gnd_vel(); |
starling | 0:8f5db5085df7 | 148 | if (saved_velocity > 0) |
starling | 0:8f5db5085df7 | 149 | { |
starling | 0:8f5db5085df7 | 150 | motor->setVelocity(saved_velocity); |
starling | 0:8f5db5085df7 | 151 | if (abs(saved_velocity) > MINIMUM_VELOCITY) |
starling | 0:8f5db5085df7 | 152 | brake = false; |
starling | 0:8f5db5085df7 | 153 | } |
starling | 0:8f5db5085df7 | 154 | break; |
starling | 0:8f5db5085df7 | 155 | case JOG_VEL: |
starling | 0:8f5db5085df7 | 156 | led->set_leds_color_nxp(LED::WHITE); |
starling | 0:8f5db5085df7 | 157 | float p, r; |
starling | 0:8f5db5085df7 | 158 | rcv.get_jog_vel(&p, &r); |
starling | 0:8f5db5085df7 | 159 | if (p == 0 || r == 0) |
starling | 0:8f5db5085df7 | 160 | motor->stopJogging(); |
starling | 0:8f5db5085df7 | 161 | else |
starling | 0:8f5db5085df7 | 162 | motor->startJogging(r, p); |
starling | 0:8f5db5085df7 | 163 | break; |
starling | 0:8f5db5085df7 | 164 | case PID_PARAMS: |
starling | 0:8f5db5085df7 | 165 | led->set_leds_color_nxp(LED::WHITE); |
starling | 0:8f5db5085df7 | 166 | float arr[4]; |
starling | 0:8f5db5085df7 | 167 | rcv.get_pid_params(arr); |
starling | 0:8f5db5085df7 | 168 | pidController->setPIDConstants(arr[0], arr[1], arr[2], arr[3]); |
starling | 0:8f5db5085df7 | 169 | |
starling | 0:8f5db5085df7 | 170 | break; |
starling | 0:8f5db5085df7 | 171 | case MAG_CALIB: |
starling | 0:8f5db5085df7 | 172 | led->set_leds_color_nxp(LED::BLUE); |
starling | 0:8f5db5085df7 | 173 | float mag[4]; |
starling | 0:8f5db5085df7 | 174 | rcv.get_mag_calib(mag); |
starling | 0:8f5db5085df7 | 175 | max_x = mag[1]; |
starling | 0:8f5db5085df7 | 176 | max_y = mag[3]; |
starling | 0:8f5db5085df7 | 177 | min_x = mag[0]; |
starling | 0:8f5db5085df7 | 178 | min_y = mag[2]; |
starling | 0:8f5db5085df7 | 179 | //printf(" max_x = %f, max_y= %f, min_x= %f, min_y= %f\r\n",mag[1],mag[3],mag[0],mag[2]); |
starling | 0:8f5db5085df7 | 180 | break; |
starling | 0:8f5db5085df7 | 181 | case LED_ON: |
starling | 0:8f5db5085df7 | 182 | led->set_leds_color_nxp(LED::WHITE); |
starling | 0:8f5db5085df7 | 183 | led->set_leds_color_main(LED::WHITE); |
starling | 0:8f5db5085df7 | 184 | main_led = 1; |
starling | 0:8f5db5085df7 | 185 | break; |
starling | 0:8f5db5085df7 | 186 | case LED_OFF: |
starling | 0:8f5db5085df7 | 187 | led->turn_leds_off_nxp(); |
starling | 0:8f5db5085df7 | 188 | led->turn_leds_off_main(); |
starling | 0:8f5db5085df7 | 189 | main_led = 0; |
starling | 0:8f5db5085df7 | 190 | break; |
starling | 0:8f5db5085df7 | 191 | default: |
starling | 0:8f5db5085df7 | 192 | //ser.printf("unknown command!\r\n"); |
starling | 0:8f5db5085df7 | 193 | } |
starling | 0:8f5db5085df7 | 194 | } |
starling | 0:8f5db5085df7 | 195 | |
starling | 0:8f5db5085df7 | 196 | /* Brake function, braking while the gyroscope is still integrating will cause considerably error in the measurement. */ |
starling | 0:8f5db5085df7 | 197 | /* Function to normalize the magnetometer reading */ |
starling | 0:8f5db5085df7 | 198 | void magCal() |
starling | 0:8f5db5085df7 | 199 | { |
starling | 0:8f5db5085df7 | 200 | //red_led = 0; |
starling | 0:8f5db5085df7 | 201 | printf("Starting Calibration"); |
starling | 0:8f5db5085df7 | 202 | mag.enable(); |
starling | 0:8f5db5085df7 | 203 | wait(0.01); |
starling | 0:8f5db5085df7 | 204 | mag.getAxis(mag_data); |
starling | 0:8f5db5085df7 | 205 | float x0 = max_x = min_y = mag_data.x; |
starling | 0:8f5db5085df7 | 206 | float y0 = max_y = min_y = mag_data.y; |
starling | 0:8f5db5085df7 | 207 | bool began = false; |
starling | 0:8f5db5085df7 | 208 | while (!(began && abs(mag_data.x - x0) < END_THRESH && abs(mag_data.y - y0) < END_THRESH)) |
starling | 0:8f5db5085df7 | 209 | { |
starling | 0:8f5db5085df7 | 210 | mag.getAxis(mag_data); |
starling | 0:8f5db5085df7 | 211 | if (mag_data.x > max_x) |
starling | 0:8f5db5085df7 | 212 | max_x = mag_data.x; |
starling | 0:8f5db5085df7 | 213 | if (mag_data.y > max_y) |
starling | 0:8f5db5085df7 | 214 | max_y = mag_data.y; |
starling | 0:8f5db5085df7 | 215 | if (mag_data.y < min_y) |
starling | 0:8f5db5085df7 | 216 | min_y = mag_data.y; |
starling | 0:8f5db5085df7 | 217 | if (mag_data.x < min_x) |
starling | 0:8f5db5085df7 | 218 | min_x = mag_data.x; |
starling | 0:8f5db5085df7 | 219 | if (abs(mag_data.x - x0) > START_THRESH && abs(mag_data.y - y0) > START_THRESH) |
starling | 0:8f5db5085df7 | 220 | began = true; |
starling | 0:8f5db5085df7 | 221 | printf("began: %d X-X0: %f , Y-Y0: %f \n\r", began, abs(mag_data.x - x0), abs(mag_data.y - y0)); |
starling | 0:8f5db5085df7 | 222 | } |
starling | 0:8f5db5085df7 | 223 | printf("Calibration Completed: X_MAX = %f , Y_MAX = %f , X_MIN = %f and Y_MIN = %f \n\r", max_x, max_y, min_x, min_y); |
starling | 0:8f5db5085df7 | 224 | } |
starling | 0:8f5db5085df7 | 225 | |
starling | 0:8f5db5085df7 | 226 | /* Function to transform the magnetometer reading in angle(rad/s).*/ |
starling | 0:8f5db5085df7 | 227 | float processMagAngle() |
starling | 0:8f5db5085df7 | 228 | { |
starling | 0:8f5db5085df7 | 229 | // printf("starting ProcessMagAngle()\n\r"); |
starling | 0:8f5db5085df7 | 230 | float mag_lpf = 0; |
starling | 0:8f5db5085df7 | 231 | Timer t1; |
starling | 0:8f5db5085df7 | 232 | for (int i = 0; i < 20; i++) |
starling | 0:8f5db5085df7 | 233 | { |
starling | 0:8f5db5085df7 | 234 | //printf("\r\n"); |
starling | 0:8f5db5085df7 | 235 | //wait(0.1); |
starling | 0:8f5db5085df7 | 236 | t1.start(); |
starling | 0:8f5db5085df7 | 237 | __disable_irq(); |
starling | 0:8f5db5085df7 | 238 | mag.getAxis(mag_data); |
starling | 0:8f5db5085df7 | 239 | __enable_irq(); |
starling | 0:8f5db5085df7 | 240 | t1.stop(); |
starling | 0:8f5db5085df7 | 241 | x = 2 * (mag_data.x - min_x) / float(max_x - min_x) - 1; |
starling | 0:8f5db5085df7 | 242 | y = 2 * (mag_data.y - min_y) / float(max_y - min_y) - 1; |
starling | 0:8f5db5085df7 | 243 | mag_lpf = mag_lpf + (-atan2(y, x)); |
starling | 0:8f5db5085df7 | 244 | wait(0.015); |
starling | 0:8f5db5085df7 | 245 | } |
starling | 0:8f5db5085df7 | 246 | // wait(20*0.01); |
starling | 0:8f5db5085df7 | 247 | // printf("Finished ProcessMagAngle() %d \n\r",t1.read_us()); |
starling | 0:8f5db5085df7 | 248 | return mag_lpf / 20; |
starling | 0:8f5db5085df7 | 249 | } |