De hele robot in 1 keer bam
Dependencies: mbed QEI Servo HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 47:63b5ccd969e9
- Parent:
- 46:8a8fa8e602a1
- Child:
- 48:31676da4be7a
diff -r 8a8fa8e602a1 -r 63b5ccd969e9 main.cpp --- a/main.cpp Thu Oct 31 14:37:21 2019 +0000 +++ b/main.cpp Thu Oct 31 16:25:32 2019 +0000 @@ -23,6 +23,7 @@ InterruptIn button2(D10); InterruptIn switch2(SW2); InterruptIn switch3(SW3); +DigitalIn extButton1(D2); //Servo button // LEDs DigitalOut led_g(LED_GREEN); @@ -49,7 +50,8 @@ FastPWM motor2Power(D5); // Servo -Servo myServo(D3); +Servo myservo(D3); +bool butt1; /* ------------------------------ INITIALIZE TICKERS, TIMERS & TIMEOUTS ------------------------------ @@ -477,7 +479,7 @@ int encoder_res = 64; QEI encoder1(D9, D8, NC, encoder_res, QEI::X4_ENCODING); //Encoder of motor 1 -QEI encoder2(D13, D12, NC, encoder_res, QEI::X4_ENCODING); //Encoder of motor 2 +QEI encoder2(D12, D13, NC, encoder_res, QEI::X4_ENCODING); //Encoder of motor 2 // Initialize variables for encoder reading volatile int counts1; @@ -496,7 +498,7 @@ const float PWM_period = 1/(18*10e3); // 18000 Hz // Important constants -const double pi = 3.1415926535897; // pi +const double pi = 3.141592; // pi const double pi2 = pi * 2; // 2 pi const double deg2rad = pi / 180; //Conversion factor degree to rad const double rad2deg = 180 / pi; //Conversion factor rad to degree @@ -522,11 +524,11 @@ float ye; // Endpoint y position [cm] // Motor angles in starting position -const float motor1_init = q1 * gearratio1; // Measured angle motor 1 in initial (starting) position +const float motor1_init = ( q1 + q2 ) * gearratio1; // Measured angle motor 1 in initial (starting) position float motor1_ref = motor1_init; // Expected motor angle float motor1_angle = motor1_init; // Actual motor angle -const float motor2_init = q2 * gearratio2; // Measured angle motor 2 in initial (starting) position +const float motor2_init = q1 * gearratio2; // Measured angle motor 2 in initial (starting) position float motor2_ref = motor2_init; float motor2_angle = motor2_init; @@ -541,7 +543,7 @@ float omega2; bool motordir2; double controlsignal2; -float motor2error; +float motor2_error; // Initialize variables for PID controller float Kp = 0.27; // Proportional gain @@ -601,19 +603,19 @@ // Motor 2 static float error_integral2 = 0; - static float e_prev2 = motor2error; + static float e_prev2 = motor2_error; //Proportional part: - u_p2 = Kp * motor2error; + u_p2 = Kp * motor2_error; //Integral part error_integral2 = error_integral2 + ei2 * Ts; u_i2 = Ki * error_integral2; //Derivative part - float error_derivative2 = (motor2error - e_prev2) / Ts; + float error_derivative2 = (motor2_error - e_prev2) / Ts; float u_d2 = Kd * error_derivative2; - e_prev2 = motor2error; + e_prev2 = motor2_error; // Sum and limit up2 = u_p2 + u_i2 + u_d2; @@ -628,7 +630,7 @@ // To prevent windup ux2 = up2 - controlsignal2; ek2 = Ka * ux2; - ei2 = motor2error - ek2; + ei2 = motor2_error - ek2; } void RKI() @@ -642,8 +644,8 @@ xe = l1 * cos(q1) + l2 * cos(q1+q2); ye = l1 * sin(q1) + l2 * sin(q1+q2); - if ( q1 < -5.0f ) { - q1 = -5.0; + if ( q1 < -5.0f * deg2rad) { + q1 = -5.0 * deg2rad; } else if ( q1 > 65.0f*deg2rad ) { q1 = 65.0f * deg2rad; } else { @@ -665,7 +667,7 @@ void motorControl() { counts1 = counts1af - counts1offset; - motor1_angle = (counts1 * encoder_factorin / gearbox_ratio) + (motor1_init + motor2_init); // Angle of motor shaft in rad + correctie voor q1 en q2 + motor1_angle = (counts1 * encoder_factorin / gearbox_ratio) + motor1_init; // Angle of motor shaft in rad + correctie voor q1 en q2 omega1 = deltaCounts1 / Ts * encoder_factorin / gearbox_ratio; // Angular velocity of motor shaft in rad/s motor1_error = motor1_ref - motor1_angle; if ( controlsignal1 < 0 ) { @@ -678,9 +680,9 @@ motor1Direction = motordir1; counts2 = counts2af - counts2offset; - motor2_angle = (counts2 * encoder_factorin / gearbox_ratio) + motor1_init; // Angle of motor shaft in rad + correctie voor q1 + motor2_angle = (counts2 * encoder_factorin / gearbox_ratio) + motor2_init; // Angle of motor shaft in rad + correctie voor q1 omega2 = deltaCounts2 / Ts * encoder_factorin / gearbox_ratio; // Angular velocity of motor shaft in rad/s - motor2error = motor2_ref-motor2_angle; + motor2_error = motor2_ref-motor2_angle; if ( controlsignal2 < 0 ) { motordir2 = 0; } else { @@ -692,6 +694,17 @@ motor2Direction = motordir2; } +void changeservo() +{ + butt1= extButton1.read(); + if (butt1==1){ + myservo.SetPosition(2000); + } + else { + myservo.SetPosition(1000); + } +} + void motorKillPower() { motor1Power.write(0.0f); @@ -774,12 +787,12 @@ void toggleServo() { if ( operation_showcard == true ) { - myServo.SetPosition(2000); + myservo.SetPosition(2000); operation_showcard = !operation_showcard; } else { - myServo.SetPosition(1000); + myservo.SetPosition(1000); operation_showcard = !operation_showcard; } } @@ -1297,6 +1310,7 @@ { sampleSignals(); global_state_machine(); + changeservo(); // controller(); // outputToMotors(); } @@ -1334,6 +1348,7 @@ button2.fall( &button2Press ); switch2.fall( &switch2Press ); switch3.fall( &switch3Press ); + extButton1.mode(PullUp); // ---------- Attach PWM ---------- motor1Power.period(PWM_period); @@ -1346,8 +1361,12 @@ while(true) { pc.printf("Global state: %i EMG state: %i Motor state: %i Operation state: %i Demo state: %i\r\n", global_curr_state, emg_curr_state, motor_curr_state, operation_curr_state, demo_curr_state); - pc.printf("EMG1 direction: %f EMG2 direction: %f \r\n", emg1_dir, emg2_dir); - //pc.printf("q1: %f q2: %f \r\n",q1*rad2deg,q2*rad2deg); + //pc.printf("EMG1 direction: %f EMG2 direction: %f \r\n", emg1_dir, emg2_dir); + pc.printf("q1: %f q2: %f \r\n",q1*rad2deg,q2*rad2deg); + pc.printf("Motor1ref: %f Motor1angle: %f\r\n",motor1_ref,motor1_angle); + pc.printf("Motor2ref: %f Motor2angle: %f\r\n",motor2_ref,motor2_angle); + pc.printf("Counts2: %i Counts2af: %i Counts2offset: %i \r\n", counts2, counts2af, counts2offset); + //pc.printf("Xe: %f Ye: %f\r\n",xe,ye); wait(0.5f); }