Moet dit er bij
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
main.cpp
- Committer:
- BasB
- Date:
- 2019-10-30
- Revision:
- 16:6334ad516980
- Parent:
- 15:75070aedf4b7
File content as of revision 16:6334ad516980:
#include "mbed.h" #include "HIDScope.h" #include "QEI.h" #include "MODSERIAL.h" #include "BiQuad.h" #include "FastPWM.h" // Button and potmeter1 control InterruptIn button1(D11); InterruptIn button2(D10); InterruptIn buttonsw2(SW2); InterruptIn buttonfailure(SW3); AnalogIn potmeter1(A0); AnalogIn potmeter2(A1); AnalogIn potmeter3(A2); AnalogIn potmeter4(A3); // Encoder DigitalIn encA1(D9); DigitalIn encB1(D8); DigitalIn encA2(D13); DigitalIn encB2(D13); QEI encoder1(D9,D8,NC,64,QEI::X4_ENCODING); //Encoding motor 1 QEI encoder2(D13,D12,NC,64,QEI::X4_ENCODING); //Encoding motor 2 float Ts = 0.002; //Sample time float deg2rad=0.0174532; //Conversion factor degree to rad float rad2deg=57.29578; //Conversion factor rad to degree float motor1angle=(-140.0-10.0)*deg2rad*5.5; //Measured angle motor 1 float motor2angle=(-10.0)*deg2rad*2.75; //Measured angle motor 2 float motor1offset; //Offset bij calibratie float motor2offset; float potmeter; float omega1; //velocity rad/s motor 1 float omega2; //Velocity rad/s motor2 // Motor DigitalOut motor2Direction(D4); FastPWM motor2Power(D5); DigitalOut motor1Direction(D7); FastPWM motor1Power(D6); //Motorcontrol bool motordir1; bool motordir2; float motor1ref=(-140.0-10.0)*deg2rad*5.5; float motor2ref=(-10.0)*deg2rad*2.75; double controlsignal1; double controlsignal2; double pi2= 6.283185; float motor1error; //motor 1 error float motor2error; float Kp=0.27; float Ki=0.35; float Kd=0.1; float u_p1; float u_p2; float u_i1; float u_i2; //Windup control float ux1; float ux2; float up1; //Proportional contribution motor 1 float up2; //Proportional contribution motor 2 float ek1; float ek2; float ei1= 0.0; //Error integral motor 1 float ei2=0.0; //Error integral motor 2 float Ka= 1.0; //Integral windup gain //RKI float Vx=0.0; //Desired linear velocity x direction float Vy=0.0; //Desired linear velocity y direction float q1=-10.0f*deg2rad; //Angle of first joint [rad] float q2=-140.0f*deg2rad; //Angle of second joint [rad] float q1dot; //Velocity of first joint [rad/s] float q2dot; //Velocity of second joint [rad/s] float l1=26.0; //Distance base-link [cm] float l2=62.0; //Distance link-endpoint [cm] float xe; //Endpoint x position [cm] float ye; //Endpoint y position [cm] //Hidscope HIDScope scope(6); //Going to send x channels of data. To access data go to 'http:/localhost:18082/' after starting HIDScope application. //State maschine enum Motor_States{motor_wait , motor_encoderset , motor_wait2 , motor_bewegen , failuremode}; Motor_States motor_curr_state; bool motor_state_changed = true; bool motor_calibration_done = false; bool motor_RKI=false; bool button1_pressed = false; bool button2_pressed = false; // PC connection MODSERIAL pc(USBTX, USBRX); // Intializing tickers Ticker motorTicker; Ticker controlTicker; Ticker directionTicker; Ticker encoderTicker; Ticker scopeTicker; Ticker tickGlobal; //Global ticker const float PWM_period = 1/(18*10e3); volatile int counts1; // Encoder counts volatile int counts2; volatile int counts1af; volatile int counts2af; int counts1offset; int counts2offset; volatile int countsPrev1 = 0; volatile int countsPrev2 = 0; volatile int deltaCounts1; volatile int deltaCounts2; float factorin = 6.23185/64; // Convert encoder counts to angle in rad float gearratio = 131.25; // Gear ratio of gearbox void button1Press() { button1_pressed = true; } void button2Press() { button2_pressed = true; } // Ticker Functions void readEncoder() { counts1af = encoder1.getPulses(); deltaCounts1 = counts1af - countsPrev1; countsPrev1 = counts1af; counts2af = encoder2.getPulses(); deltaCounts2 = counts2af - countsPrev2; countsPrev2 = counts2af; } void PID_controller(){ static float error_integral1=0; static float e_prev1=motor1error; //Proportional part: u_p1=Kp*motor1error; //Integral part error_integral1=error_integral1+ei1*Ts; u_i1=Ki*error_integral1; //Derivative part float error_derivative1=(motor1error-e_prev1)/Ts; float u_d1=Kd*error_derivative1; e_prev1=motor1error; // Sum and limit up1= u_p1+u_i1+u_d1; if (up1>1){ controlsignal1=1;} else if (up1<-1){ controlsignal1=-1;} else { controlsignal1=up1;} // To prevent windup ux1= up1-controlsignal1; ek1= Ka*ux1; ei1= motor1error-ek1; // Motor 2 static float error_integral2=0; static float e_prev2=motor2error; //Proportional part: u_p2=Kp*motor2error; //Integral part error_integral2=error_integral2+ei2*Ts; u_i2=Ki*error_integral2; //Derivative part float error_derivative2=(motor2error-e_prev2)/Ts; float u_d2=Kd*error_derivative2; e_prev2=motor2error; // Sum and limit up2= u_p2+u_i2+u_d2; if (up2>1.0f){ controlsignal2=1.0f;} else if (up2<-1){ controlsignal2=-1.0f;} else { controlsignal2=up2;} // To prevent windup ux2= up2-controlsignal2; ek2= Ka*ux2; ei2= motor2error-ek2; } void RKI(){ q1dot=(l2*cos(q1+q2)*Vx+l2*sin(q1+q2)*Vy)/((-l1*sin(q1)-l2*sin(q1+q2))*l2*cos(q1+q2)+l2*sin(q1+q2)*(l1*cos(q1)+l2*cos(q1+q2))); q2dot=((-l1*cos(q1)-l2*cos(q1+q2))*Vx+(-l1*sin(q1)-l2*sin(q1+q2))*Vy)/((-l1*sin(q1)-l2*sin(q1+q2))*l2*cos(q1+q2)+l2*sin(q1+q2)*(l1*cos(q1)+l2*cos(q1+q2))); q1=q1+q1dot*Ts; q2=q2+q2dot*Ts; xe=l1*cos(q1)+l2*cos(q1+q2); ye=l1*sin(q1)+l2*sin(q1+q2); if (q1<-5.0f){ q1=-5.0;} else if (q1>65.0f*deg2rad){ q1=65.0f*deg2rad;} else{ q1=q1;} if (q2>-50.0*deg2rad){ q2=-50.0*deg2rad;} else if (q2<-138.0*deg2rad){ q2=-138.0*deg2rad;} else{ q2=q2;} motor1ref=5.5f*q1+5.5f*q2; motor2ref=2.75f*q1; } void motorControl(){ counts1= counts1af-counts1offset; motor1angle = (counts1 * factorin / gearratio)-((140.0*5.5*deg2rad)+(10.0*5.5*deg2rad)); // Angle of motor shaft in rad + correctie voor q1 en q2 omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s motor1error=motor1ref-motor1angle; if (controlsignal1<0){ motordir1= 0;} else { motordir1= 1;} motor1Power.write(abs(controlsignal1)); motor1Direction= motordir1; counts2= counts2af - counts2offset; motor2angle = (counts2 * factorin / gearratio)-(10.0*2.75*deg2rad); // Angle of motor shaft in rad + correctie voor q1 omega2 = deltaCounts2 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s motor2error=motor2ref-motor2angle; if (controlsignal2<0){ motordir2= 0;} else { motordir2= 1;} if (motor_calibration_done == true){ motor2Power.write(abs(controlsignal2));} motor2Direction= motordir2; } void do_motor_wait(){ // Entry function if ( motor_state_changed == true ) { motor_state_changed = false; // More functions } PID_controller(); motorControl(); // State transition guard if ( button2_pressed ) { button2_pressed = false; motor_curr_state = motor_encoderset; //Beginnen met calibratie motor_state_changed = true; // More functions } } void do_motor_Encoder_Set(){ // Entry function if ( motor_state_changed == true ) { motor_state_changed = false; // More functions } motor1Power.write(0.0f); motor2Power.write(0.0f); counts1offset = counts1af ; counts2offset = counts2af; // State transition guard if ( button2_pressed ) { button2_pressed = false; motor_calibration_done = true; motor_curr_state = motor_wait2; motor_state_changed = true; } } void do_motor_wait2(){ // Entry function if ( motor_state_changed == true ) { motor_state_changed = false; // More functions } PID_controller(); motorControl(); RKI(); // Do nothing until end condition is met // State transition guard if ( button2_pressed ) { button2_pressed = false; motor_curr_state = motor_bewegen; motor_state_changed = true; // More functions } } void do_motor_bewegen(){ // Entry function if ( motor_state_changed == true ) { motor_state_changed = false; // More functions } static float t=0; Vy=10.0f*sin(1.0f*t); Vx=0.0f; t+=Ts; PID_controller(); motorControl(); RKI(); if ( button2_pressed ) { button2_pressed = false; motor_RKI = false; motor_curr_state = motor_wait2; motor_state_changed = true; // More functions } } void do_failuremode(){ // Entry function if ( motor_state_changed == true ) { motor_state_changed = false; // More functions } motor_curr_state = failuremode; motor1Power.write(0.0f); motor2Power.write(0.0f); Vx=0.0f; Vy=0.0f; } void motor_state_machine() { switch(motor_curr_state) { case motor_wait: do_motor_wait(); break; case motor_encoderset: do_motor_Encoder_Set(); break; case motor_wait2: do_motor_wait2(); break; case motor_bewegen: do_motor_bewegen(); break; } } // Global loop of program void tickGlobalFunc() { //sampleSignal(); //emg_state_machine(); motor_state_machine(); readEncoder(); // controller(); // outputToMotors(); } int main() { pc.baud(115200); pc.printf("\r\nStarting...\r\n\r\n"); motor1Power.period(PWM_period); motor2Power.period(PWM_period); motor_curr_state = motor_wait; // Start off in EMG Wait state tickGlobal.attach( &tickGlobalFunc, Ts ); button1.fall(&button1Press); button2.fall(&button2Press); buttonfailure.fall(&do_failuremode); while (true) { pc.printf("counts1offset %i counts1af: %i counts1: %i\r\n",counts1offset,counts1af,counts1); pc.printf("counts2offset %i counts2af: %i counts2: %i\r\n",counts2offset,counts2af,counts2); pc.printf("state: %i motor1ref: %f motor1angle: %f\r\n",motor_curr_state,motor1ref,motor1angle); pc.printf("q1: %f q2: %f motor1error: %f \r\n",q1*rad2deg,q2*rad2deg, motor1error); pc.printf("Xe: %f Ye: %f\r\n",xe,ye); wait(0.5); } }