Diff: main.cpp

Revision:

47:63b5ccd969e9

Parent:

46:8a8fa8e602a1

Child:

48:31676da4be7a

--- 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);
     }