TrekkingPhoenix / Mbed 2 deprecated Trekking_Controller_refac

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?

UserRevisionLine numberNew 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 }