Ironcup Mar 2020
Dependencies: mbed mbed-rtos MotionSensor EthernetInterface
Diff: main.cpp
- Revision:
- 20:7138ab2f93f7
- Parent:
- 18:c1cd11db47ed
- Child:
- 22:b7cca3089dfe
--- a/main.cpp Sun May 15 19:14:06 2016 +0000 +++ b/main.cpp Sat Jul 16 19:17:08 2016 +0000 @@ -4,22 +4,23 @@ #include "CarPWM.h" #include "receiver.h" #include "Motor.h" +#include "rtos.h" -#define PI 3.141592653589793238462 -#define Ts 0.02 // Seconds +#define PI 3.141593 +#define Ts 0.02 // Controller period(Seconds) #define PWM_PERIOD 13.5 // ms #define INITIAL_P 0.452531214933414 #define INITIAL_I 5.45748932024049 #define INITIAL_D 0.000233453623255507 #define INITIAL_N 51.0605584484153 -#define BRAKE_CONSTANT 40 -#define BRAKE_WAIT 0.3 -#define END_THRESH 4 -#define START_THRESH 10 +#define END_THRESH 4 //For magnetometer calibration +#define START_THRESH 10 //For magnetometer calibration #define MINIMUM_VELOCITY 15 -#define GYRO_PERIOD 5000 //us -#define LED_ON 0 -#define LED_OFF 1 +#define MINIMUM_CURVE_VELOCITY 19 +#define ERROR_THRESH PI/5 +#define GYRO_PERIOD 15000 //us +#define RGB_LED_ON 0 //active Low +#define RGB_LED_OFF 1 //active Low #define MIN -1.5 #define MAX 1.5 @@ -34,59 +35,19 @@ AQUA, WHITE}; -void turn_leds_off(DigitalOut& red, DigitalOut& green, DigitalOut& blue) -{ - red = LED_OFF; - green = LED_OFF; - blue = LED_OFF; -} -void set_leds_color(int color, DigitalOut& red, DigitalOut& green, DigitalOut& blue) -{ - turn_leds_off(red, green, blue); - - switch(color) - { - case RED: - red = LED_ON; - break; - case GREEN: - green = LED_ON; - break; - case BLUE: - blue = LED_ON; - break; - case PURPLE: - red = LED_ON; - blue = LED_ON; - break; - case YELLOW: - red = LED_ON; - green = LED_ON; - break; - case AQUA: - blue = LED_ON; - green = LED_ON; - break; - case WHITE: - red = LED_ON; - green = LED_ON; - blue = LED_ON; - break; - default: - break; - } -} - -Serial ser(USBTX, USBRX); // Initialize Serial port +//Control Objetcs PwmOut servo(PTD3); // Servo connected to pin PTD3 Motor motor; FXOS8700Q_mag mag(PTE25,PTE24,FXOS8700CQ_SLAVE_ADDR1); FXOS8700Q_acc acc( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1); FXAS21002 gyro(PTE25,PTE24); +//Leds Objects DigitalOut red_led(LED_RED); DigitalOut green_led(LED_GREEN); DigitalOut blue_led(LED_BLUE); +DigitalOut main_led(PTD2); +//Protocol Objects Receiver rcv; EthernetInterface eth; @@ -98,18 +59,29 @@ void control(); Ticker controller_ticker; -// Magnetometer variables and functions +// Motor and servo variables +float saved_velocity = 0; +bool brake = false; + +// Magnetometer/Gyro variables and functions float max_x=0, max_y=0, min_x=0, min_y=0,x,y; MotionSensorDataUnits mag_data; MotionSensorDataCounts mag_raw; float processMagAngle(); +float gyro_reference = 0; void magCal(); // Protocol void readProtocol(); +int contp = 0; // for debug only + +// NXP RGB_LEDs control +void set_leds_color(int color); +void turn_leds_off(); int main(){ // Initializing sensors: + main_led = 1; acc.enable(); gyro.gyro_config(MODE_1); initializeController(); @@ -117,19 +89,22 @@ // Set initial control configurations motor.setVelocity(0); - // Protocol parameters + // Protocol initialization + printf("Initializing Ethernet!\r\n"); - set_leds_color(RED, red_led, green_led, blue_led); + set_leds_color(RED); eth.init(RECEIVER_IFACE_ADDR, RECEIVER_NETMASK_ADDR, RECEIVER_GATEWAY_ADDR); eth.connect(); printf("Protocol initialized! \r\n"); - set_leds_color(BLUE, red_led, green_led, blue_led); + set_leds_color(BLUE); rcv.set_socket(); gyro.start_measure(GYRO_PERIOD); + main_led = 0; controller_ticker.attach(&control,Ts); //main loop while(1){ readProtocol(); + // printf("%f \r\n",gyro.get_angle()); } } void control(){ @@ -139,77 +114,109 @@ if(!rcv.receive()) return; char msg = rcv.get_msg(); - printf("Message received!"); + //printf("Message received!"); + //contp++; + //printf(" %d \r\n",contp); switch(msg) { case NONE: - //ser.printf("sending red signal to led\r\n"); - set_leds_color(BLACK,red_led,green_led,blue_led); - printf("NONE\r\n"); - wait(1); break; case BRAKE: - //ser.printf("sending green signal to led\r\n"); - set_leds_color(YELLOW,red_led,green_led,blue_led); - motor.stopJogging(); - printf("BRAKE\r\n"); - motor.brakeMotor(); + //printf("BRAKE "); + float intensity, b_wait; + rcv.get_brake(&intensity,&b_wait); + if(!brake){ + set_leds_color(YELLOW); + motor.stopJogging(); + // printf("BRAKE\r\n"); + setServoPWM(0,servo); + //reference = 0; + controller_ticker.detach(); + motor.brakeMotor(intensity,b_wait); + controller_ticker.attach(&control,Ts); + saved_velocity = 0; + brake = true; + } + break; + case GYRO_ZERO: + gyro.stop_measure(); + wait(0.05); + gyro.start_measure(GYRO_PERIOD); + break; + case ANG_SET: + set_leds_color(PURPLE); + //printf("ANG_SET\r\n"); + gyro_reference = gyro.get_angle(); + initializeController(); break; case ANG_RST: - //ser.printf("sending blue signal to led\r\n"); - set_leds_color(PURPLE,red_led,green_led,blue_led); - printf("ANG_RST\r\n"); - gyro.stop_measure(); - printf("Stopped gyro\r\n"); - gyro.start_measure(GYRO_PERIOD); - printf("Starting Gyro\r\n"); + //printf("ANG_RST\r\n"); + gyro_reference = 0; + set_leds_color(PURPLE); initializeController(); break; - case ANG_REF: - set_leds_color(GREEN,red_led,green_led,blue_led); - reference = rcv.get_ang_ref() - processMagAngle(); - printf("New reference: %f \n\r",reference*180/PI); + case MAG_ANG_REF: + set_leds_color(BLUE); + reference = rcv.get_mag_ang_ref() - processMagAngle(); + //printf("New reference: %f \n\r",reference*180/PI); if(reference > PI) reference = reference - 2*PI; else if(reference < -PI) reference = reference + 2*PI; break; - case CAM_ANG_REF: - set_leds_color(RED,red_led,green_led,blue_led); - reference = rcv.get_cam_ang_ref(); - printf("New reference: %f \n\r",reference*180/PI); + case ABS_ANG_REF: + set_leds_color(GREEN); + reference = rcv.get_abs_ang_ref(); + //printf("New reference: %f \n\r",reference*180/PI); + if(reference > PI) + reference = reference - 2*PI; + else if(reference < -PI) + reference = reference + 2*PI; + break; + case REL_ANG_REF: + set_leds_color(RED); + reference = rcv.get_rel_ang_ref() + gyro.get_angle()*PI/180; + //printf("New reference: %f \n\r",reference*180/PI); + if(reference > PI) + reference = reference - 2*PI; + else if(reference < -PI) + reference = reference + 2*PI; break; case GND_VEL: - set_leds_color(AQUA,red_led,green_led,blue_led); - float vel = rcv.get_gnd_vel(); - motor.setVelocity(vel); - printf("GND_VEL = %f\r\n", vel); + set_leds_color(AQUA); + saved_velocity = rcv.get_gnd_vel(); + //printf("GND_VEL"); + if(saved_velocity > 0){ + motor.setVelocity(saved_velocity); + if(abs(saved_velocity) > MINIMUM_VELOCITY) + brake = false; + //printf("GND_VEL = %f\r\n", saved_velocity); + } break; case JOG_VEL: - set_leds_color(WHITE,red_led,green_led,blue_led); + set_leds_color(WHITE); float p, r; rcv.get_jog_vel(&p,&r); - printf("Joggin with period %f and duty cycle %f\r\n",p,r); + //printf("Joggin with period %f and duty cycle %f\r\n",p,r); if(p == 0 || r == 0) motor.stopJogging(); else motor.startJogging(r,p); break; case PID_PARAMS: - set_leds_color(RED,red_led,green_led,blue_led); - float ar[4]; - rcv.get_pid_params(ar); - P = ar[0]; - I = ar[1]; - D = ar[2]; - N = ar[3]; - printf("PID_PARAMS | kp=%f, ki=%f, kd=%f, n=%f\r\n", - ar[0], ar[1], ar[2], ar[3]); + set_leds_color(WHITE); + float arr[4]; + rcv.get_pid_params(arr); + P = arr[0]; + I = arr[1]; + D = arr[2]; + N = arr[3]; + // printf("PID_PARAMS | kp=%f, ki=%f, kd=%f, n=%f\r\n", + // arr[0], arr[1], arr[2], arr[3]); - wait(1); break; case MAG_CALIB: - set_leds_color(BLUE,red_led,green_led,blue_led); + set_leds_color(BLUE); printf("MAG_CALIB\r\n"); float mag[4]; rcv.get_mag_calib(mag); @@ -217,10 +224,17 @@ max_y=mag[3]; min_x=mag[0]; min_y=mag[2]; - printf(" max_x = %f, max_y= %f, min_x= %f, min_y= %f\r\n",mag[1],mag[3],mag[0],mag[2]); + //printf(" max_x = %f, max_y= %f, min_x= %f, min_y= %f\r\n",mag[1],mag[3],mag[0],mag[2]); + break; + case LED_ON: + set_leds_color(BLACK); + main_led = 1; + break; + case LED_OFF: + set_leds_color(BLACK); + main_led = 0; break; default: - printf("nothing understood\r\n"); //ser.printf("unknown command!\r\n"); } } @@ -240,7 +254,7 @@ /* PID controller for angular position */ void controlAnglePID(float P, float I, float D, float N){ /* Getting error */ - float feedback = gyro.get_angle(); + float feedback = gyro.get_angle() - gyro_reference; e[1] = e[0]; e[0] = reference - (feedback*PI/180); if(e[0] > PI) @@ -264,6 +278,13 @@ /** Controller **/ u = up[0] + ud[0] + ui[0]; setServoPWM(u*100/(PI/8), servo); + if(abs(e[0]) >= ERROR_THRESH && motor.getVelocity() > MINIMUM_CURVE_VELOCITY){ + saved_velocity = motor.getVelocity(); + motor.setVelocity(MINIMUM_CURVE_VELOCITY); + } + else if(abs(e[0]) < ERROR_THRESH && motor.getVelocity() != saved_velocity){ + motor.setVelocity(saved_velocity); + } } /* Brake function, braking while the gyroscope is still integrating will cause considerably error in the measurement. */ /* Function to normalize the magnetometer reading */ @@ -295,7 +316,7 @@ /* Function to transform the magnetometer reading in angle(rad/s).*/ float processMagAngle(){ - printf("starting ProcessMagAngle()\n\r"); + // printf("starting ProcessMagAngle()\n\r"); float mag_lpf = 0; Timer t1; for(int i = 0; i<20; i++){ @@ -312,6 +333,49 @@ wait(0.015); } // wait(20*0.01); - printf("Finished ProcessMagAngle() %d \n\r",t1.read_us()); + // printf("Finished ProcessMagAngle() %d \n\r",t1.read_us()); return mag_lpf/20; +} +void turn_leds_off() +{ + red_led = RGB_LED_OFF; + green_led = RGB_LED_OFF; + blue_led = RGB_LED_OFF; +} + +void set_leds_color(int color) +{ + turn_leds_off(); + + switch(color) + { + case RED: + red_led = RGB_LED_ON; + break; + case GREEN: + green_led = RGB_LED_ON; + break; + case BLUE: + blue_led = RGB_LED_ON; + break; + case PURPLE: + red_led = RGB_LED_ON; + blue_led = RGB_LED_ON; + break; + case YELLOW: + red_led = RGB_LED_ON; + green_led = RGB_LED_ON; + break; + case AQUA: + blue_led = RGB_LED_ON; + green_led = RGB_LED_ON; + break; + case WHITE: + red_led = RGB_LED_ON; + green_led = RGB_LED_ON; + blue_led = RGB_LED_ON; + break; + default: + break; + } } \ No newline at end of file