Ironcup Mar 2020
Dependencies: mbed mbed-rtos MotionSensor EthernetInterface
main.cpp
- Committer:
- drelliak
- Date:
- 2016-04-23
- Revision:
- 8:a1067fcde341
- Parent:
- 7:27516a2b504b
File content as of revision 8:a1067fcde341:
#include "FXAS21002.h" #include "FXOS8700Q.h" #include "mbed.h" #include "CarPWM.h" #include "receiver.h" #define PI 3.141592653589793238462 #define Ts 0.02 // 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 2 #define GYRO_OFFSET 0.0152 #define END_THRESH 4 #define START_THRESH 10 #define MINIMUM_VELOCITY 15 Serial ser(USBTX, USBRX); // Initialize Serial port PwmOut motor(PTD1); // Motor connected to pin PTD1 PwmOut servo(PTD3); // Servo connected to pin PTD3 FXOS8700Q_mag mag(PTE25,PTE24,FXOS8700CQ_SLAVE_ADDR1); FXAS21002 gyro(PTE25,PTE24); // PID controller parameters and functions float e[2], u, up[1],ui[2], ud[2]; // The vector coeficient means a time delay, for exemple e[a] = e(k-a) -> z^(-a)e(k) float P, I, D, N, reference = 0; void controlAnglePID(float P, float I, float D, float N); void initializeController(); // Gyroscope variables and functions float gyro_data[3], gyro_angle; Timer t; void processGyroAngle(); void startGyro(); void stopGyro(); // Magnetometer variables and functions float max_x, max_y, min_x, min_y,x,y; MotionSensorDataUnits mag_data; float processMagAngle(); void magCal(); // State variables and functions float feedback, velocity = 0, jog_duty_cycle, jog_period; bool alternate_motor = 0; void readProtocol(); void brakeMotor(); void reverseMotor(int speed); void setVelocity(int new_velocity); void motorAlternation(); Ticker interruption; // Test functions void debug(); int main(){ //printf("Initializing controller....\r\n\r\n"); initializeController(); //printf("Controller Initialized. \r\n"); //magCal(); gyro_angle = 0;//processMagAngle(); ser.baud(9600); ser.format(8,SerialBase::None,1); velocity = MINIMUM_VELOCITY; setMotorPWM(velocity,motor); startGyro(); while (true){ processGyroAngle(); controlAnglePID(P,I,D,N); if(t.read_us() < Ts*1000000) wait_us(Ts*1000000 - t.read_us()); if(ser.readable()) readProtocol(); } } void motorAlternation() { interruption.detach(); if(!alternate_motor){ setMotorPWM(velocity, motor); interruption.attach(&motorAlternation, jog_duty_cycle*jog_period); } else{ setMotorPWM(10, motor); interruption.attach(&motorAlternation, (1-jog_duty_cycle)*jog_period); } alternate_motor = !alternate_motor; } void readProtocol(){ printf("Entrei no protocolo!\n\r"); char msg = ser.getc(); printf("%d \n\r", (int)msg); switch(msg) { case NONE: //ser.printf("sending red signal to led\r\n"); return; break; case BRAKE: //ser.printf("BRAKE!r\n"); brakeMotor(); break; case ANG_RST: //ser.printf("sending blue signal to led\r\n"); stopGyro(); gyro_angle = 0; startGyro(); return; break; case ANG_REF: reference = get_ang_ref(ser); break; case GND_VEL: //ser.printf("GND_VEL\r\n"); velocity = get_gnd_vel(ser); //ser.printf("%f\r\n",velocity); //interruption.detach(); break; case PID_PARAMS: get_pid_params(ser, &P, &I, &D, &N); break; case JOG_VEL: get_jog_vel(ser,&jog_period,&jog_duty_cycle); interruption.attach(&motorAlternation, jog_duty_cycle*jog_period); break; default: ser.printf("GARBAGE\r\n"); // ser.flush(); } } /* Initialize the controller parameter P, I, D and N with the initial values and set the error and input to 0. */ void initializeController(){ for(int i =0; i<2; i++){ e[i] = 0; ui[i] = 0; ud[i] = 0; } P= INITIAL_P; I= INITIAL_I; D= INITIAL_D; N= INITIAL_N; } /* Start the Gyroscope timer and set the initial configuration */ void startGyro(){ gyro.gyro_config(); t.start(); } /* Stop and reset the Gyroscope */ void stopGyro(){ t.stop(); t.reset(); gyro_angle = 0; } /* Integrate the Gyroscope to get the angular position (Deegre/seconds) */ void processGyroAngle(){ gyro.acquire_gyro_data_dps(gyro_data); t.stop(); gyro_angle = gyro_angle + (gyro_data[2] + GYRO_OFFSET)*(double)(t.read_us())/1000000; t.reset(); t.start(); feedback = gyro_angle; if(feedback > 180) feedback = feedback - 360; if(feedback < -180) feedback = feedback + 360; } /* PID controller for angular position */ void controlAnglePID(float P, float I, float D, float N){ /* Getting error */ e[1] = e[0]; e[0] = reference - (feedback*PI/180); if(e[0] > PI) e[0]= e[0] - 2*PI; if(e[0] < -PI) e[0] = e[0] + 2*PI; if(abs(e[0]) > PI/6){ setMotorPWM(MINIMUM_VELOCITY,motor); } else{ setMotorPWM(velocity,motor); } /* Proportinal Part */ up[0] = e[0]*P; /* Integral Part */ ui[1] = ui[0]; if(abs(u) < PI/8){ ui[0] = (P*I*Ts)*e[1] + ui[1]; } else if(u > 0) ui[0] = PI/8 - up[0]; else if(u < 0) ui[0] = -PI/8 - up[0]; /* Derivative Part */ ud[1] = ud[0]; ud[0] = P*D*N*(e[0] - e[1]) - ud[1]*(N*Ts -1); /** Controller **/ u = up[0] + ud[0] + ui[0]; setServoPWM(u*100/(PI/8), servo); } /* Brake function, braking while the gyroscope is still integrating will cause considerably error in the measurement. */ void brakeMotor(){ if(velocity >= 0){ setMotorPWM(-BRAKE_CONSTANT, motor); wait(BRAKE_WAIT); velocity = 0; setMotorPWM(velocity,motor); } else { setVelocity(0); } } void reverseMotor(int speed){ for(int i=0 ; i >= -speed; i--){ setMotorPWM((float)i,motor); wait_ms(13.5); } for(int i=-speed ; i <= 0; i++){ setMotorPWM((float)i,motor); wait_ms(13.5); } for(int i=0 ; i >= -speed; i--){ setMotorPWM((float)i,motor); wait_ms(13.5); } } void setVelocity(int new_velocity){ if( velocity > new_velocity){ for(; velocity >= new_velocity; velocity--){ setMotorPWM(velocity,motor); wait_ms(PWM_PERIOD); } velocity++; } else if(velocity < new_velocity){ for(; velocity <= new_velocity; velocity++){ setMotorPWM(velocity,motor); wait_ms(PWM_PERIOD); } velocity--; } } /* Debug functions that prints the sensor and control inputs values. Since it's time consuming it should not be used */ /* in the main loop or the controller performance may be affected. */ void debug(){ //printf("ERROR: %f Up: %f Ui: %f Ud: %f U: %f \r\n", e[0]*180/3.1415, up[0]*100/(3.1415/8), ui[0]*100/(3.1415/8), ud[0]*100/(3.1415/8),u*100/(3.1415/8)); //printf("Erro: %f Sensor: %f Magnetometer: %f \r\n",e[0]*180/PI,sensor,processMagAngle(0)*180/PI); printf(" %f \r\n",feedback); } /* Function to normalize the magnetometer reading */ void magCal(){ printf("Starting Calibration"); mag.enable(); wait(0.01); mag.getAxis(mag_data); float x0 = max_x = min_y = mag_data.x; float y0 = max_y = min_y = mag_data.y; bool began = false; while(!(began && abs(mag_data.x - x0) < END_THRESH && abs(mag_data.y - y0) < END_THRESH)){ mag.getAxis(mag_data); if(mag_data.x > max_x) max_x = mag_data.x; if(mag_data.y > max_y) max_y = mag_data.y; if(mag_data.y < min_y) min_y = mag_data.y; if(mag_data.x < min_x) min_x = mag_data.x; if(abs(mag_data.x-x0)>START_THRESH && abs(mag_data.y-y0) > START_THRESH) began = true; printf("began: %d X-X0: %f , Y-Y0: %f \n\r", began, abs(mag_data.x-x0),abs(mag_data.y-y0)); } 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); } /* Function to transform the magnetometer reading in angle(rad/s).*/ float processMagAngle(){ mag.getAxis(mag_data); x = 2*(mag_data.x-min_x)/float(max_x-min_x) - 1; y = 2*(mag_data.y-min_y)/float(max_y-min_y) - 1; return atan2(y,x); }