Diff: main.cpp

Revision:

14:7a95e57b5364

Parent:

13:acd120520e80

Child:

15:207d01972e0b

--- a/main.cpp	Mon Oct 30 15:01:26 2017 +0000
+++ b/main.cpp	Mon Oct 30 16:35:20 2017 +0000
@@ -42,7 +42,7 @@
 // ----------------------------------
 
 // --- Define Ticker and Timeout ---
-Ticker mainticker;          //Ticker that calls every function. Functions are called by means of a variable named go, e.g. go_EMG
+Ticker mainticker;          //Ticker that calls every function. Functions are called by means of a variable named go, e.g. go_calibration
 Timeout calibrationgo;      // Timeout that will determine the duration of the calibration.
 // ---------------------------------
 
@@ -51,7 +51,7 @@
 
 volatile double Pos1;                        // Encoder readout of motor 1 [counts]
 volatile double Pos2;                        // Encoder readout of motor 2 [counts]
-volatile int count = 0;                         // Loop counts 
+volatile int count = 0;                         // Loop counts
 
 double Kp = 250;// you can set these constants however you like depending on trial & error
 double Ki = 100;
@@ -101,11 +101,8 @@
 
 
 // --- Booleans for the maintickerfunction ---
-//bool readoutsetpoint = true;
-bool go_EMG;                    // Boolean to put on/off the EMG readout
 bool go_calibration;            // Boolean to put on/off the calibration of the EMG
 bool go_switch;                  // Boolean to go to different state
-bool go_PID;                    // Boolean to calculate PID and motor aansturing --> probably replaced by go_move
 bool go_move;                   //Boolean to move the robot arm and base
 bool go_kinematics;               // Boolean which will determine whether a new position will be calculated or not --> replace by go_move?
 // -------------------------------------------
@@ -142,11 +139,11 @@
     emg1.filter();                      //filter the incoming emg signal
     emg2.filter();
     //emg3.filter();
-
-    /*  scope.set(0, emg1.normalized);      //views emg1.normalized on port 0 of HIDScope
-      scope.set(1, emg1.emg0);
-      scope.send();*/
-
+    /*
+        scope.set(0, emg1.normalized);      //views emg1.normalized on port 0 of HIDScope
+        scope.set(1, emg2.normalized);
+        scope.send();
+    */
 }
 
 // --- Kinematics functions ---
@@ -273,7 +270,9 @@
     Q2 += W2*T;                     // Predicted value for q2 [rad]
     Q3 += W3*T;                    // Predicted value for q3 [rad]
 
-    // pc.printf("\r\n q1 = %.3f, q2 = %.3f, q3 = %.3f\r\n", q1,q2,q3);
+    if (count == 100) {
+        //pc.printf("\r\n Q1 = %.3f, Q2 = %.3f, Q3 = %.3f\r\n", Q1,Q2,Q3);
+    }
 
 }
 
@@ -287,21 +286,18 @@
             ledblue = 1;
             ledred = 1;
             ledgreen = 0;
-            pc.printf("state is vertical");
             break;
         case R_VERTICAL:
             state = R_BASE;
             ledgreen = 1;
             ledblue = 1;
             ledred = 0;
-            pc.printf("state is base");
             break;
         case R_BASE:
             state = R_HORIZONTAL;
             ledgreen = 1;
             ledred = 1;
             ledblue = 0;
-            pc.printf("state is horizontal");
             break;
     }
     wait(1.0f);                             // waits 1 second to continue with the main function. I think ticker does run, but main is on hold.
@@ -311,7 +307,7 @@
 }
 
 // --- Determine the setpoints of the joint velocities
-void setpointreadout(float v_des)
+void setpointreadout(float v_des)  // something goes wrong here
 {
     /*
     potvalue = pot.read();
@@ -324,6 +320,17 @@
     Pos1 = motor1.getPosition()/16800*d*pi;                 // Position of link 1 [m]
     Pos2 = motor2.getPosition()/16800*2*pi;                 // Position of link 2 [m]
 
+    
+    if(pot.read()>= EMG_threshold)
+    {
+        v_des = pot.read();
+        pc.printf("potvalue is %f\r\n", v_des);
+    }
+    if (pot2.read() >= EMG_threshold)
+    {
+        v_des = -pot2.read();
+    } 
+/*
     if (emg1.normalized >= EMG_threshold && emg2.normalized <= EMG_threshold) { // Positive direction
         v_des = emg1.normalized;
     }
@@ -331,6 +338,7 @@
     if (emg1.normalized >= EMG_threshold && emg2.normalized <= EMG_threshold) { // Negative direction
         v_des = -emg2.normalized;
     }
+    */
 
 //pc.printf("\r\nv_des in setpoint function= %f\r\n", v_des);
 }
@@ -400,7 +408,9 @@
 void r_movehorizontal()
 {
     setpointreadout(Vx_des);                       // Start with obtaining the position of the robot and the desired velocities
-    //pc.printf("\r\n Vx_des outside setpoint function = %f\r\n", Vx_des);
+    if (count == 100) {
+        //pc.printf("\r\n Vx_des outside setpoint function = %f\r\n", Vx_des);
+    }
     Jacobian(Vx_des, 0, 0);                         // Give the obtained end-effector velocities as input to the Jacobian function. Output of this function is necessary joint velocities and predicted joint positions
     PIDcalculation(Q1, Q2, Q3);                      // Ensure smooth motion. Uses new position of joints calculated by Jacobian. This function then converts the position into appropriate PWM out values.
 
@@ -409,7 +419,9 @@
 void r_movevertical()
 {
     setpointreadout(Vy_des);                       // Start with obtaining the position of the robot and the desired velocities
-    //pc.printf("\r\n Vy_des outside setpoint function = %f\r\n", Vy_des);
+    if (count == 100) {
+        // pc.printf("\r\n Vy_des outside setpoint function = %f\r\n", Vy_des);
+    }
     Jacobian(0, Vy_des, 0);                         // Give the obtained end-effector velocities as input to the Jacobian function. Output of this function is necessary joint velocities and predicted joint positions
     PIDcalculation(Q1, Q2, Q3);                      // Ensure smooth motion. Uses new position of joints calculated by Jacobian. This function then converts the position into appropriate PWM out values.
 
@@ -417,12 +429,12 @@
 
 void r_movebase()
 {
-
     setpointreadout(Vz_des);                       // Start with obtaining the position of the robot and the desired velocities
-    //pc.printf("\r\n Vz_des outside setpoint function = %f\r\n", Vz_des);
+    if (count == 100) {
+        pc.printf("\r\n Vz_des outside setpoint function = %f\r\n", Vz_des);
+    }
     Jacobian(0, 0, Vz_des);                         // Give the obtained end-effector velocities as input to the Jacobian function. Output of this function is necessary joint velocities and predicted joint positions
     PIDcalculation(Q1, Q2, Q3);                      // Ensure smooth motion. Uses new position of joints calculated by Jacobian. This function then converts the position into appropriate PWM out values.
-
 }
 //--------------------------------
 
@@ -434,48 +446,41 @@
         r_processStateSwitch();
     }
 
-    if(go_EMG && count%2 == 0) {
-        processEMG();
-    }
-    
+    //if(1) {                      // Sample EMG with 500 Hz
+    processEMG();
+
+    //}
+
     if (go_move) {
-            switch(state) {
-                case R_HORIZONTAL:
-                    r_movehorizontal();
-                    break;
-                case R_VERTICAL:
-                    r_movevertical();
-                    break;
-                case R_BASE:
-                    r_movebase();
-                    break;
-            }
+        switch(state) {
+            case R_HORIZONTAL:
+                r_movehorizontal();
+                break;
+            case R_VERTICAL:
+                r_movevertical();
+                break;
+            case R_BASE:
+                r_movebase();
+                break;
         }
-
-    /*if(emg1.normalized >=0.2 && emg2.normalized >= 0.2) // PIDcalculation should not happen.
-    {
-        go_PID = false;
     }
-    else{go_PID = true;}*/
 
 
 }
 
 int main()
 {
-    pc.baud(9600);
-    go_EMG = true;                      // Setting ticker variables
+    pc.baud(115200);
     go_calibration = true;              // Setting the timeout variable
-    go_PID = true;
-    go_switch = false;
+    go_switch = false;                  // Setting ticker variables
     go_move = true;
     speed1.period(PwmPeriod);
     speed2.period(PwmPeriod);
-
+    state = R_BASE;
     calibrationgo.attach(&calibrationGO, 5.0);     // Attach calibration timeout to calibration function
     mainticker.attach(&calibrationEMG, 0.001f);      // Attach ticker to the calibration function
     wait(5.0f);
-    mainticker.attach(&maintickerfunction,0.001f);
+    mainticker.attach(&maintickerfunction,0.002f);
 
     while(true) {
 
@@ -488,11 +493,11 @@
 
         }
 
-        
+
 
         if(count == 100) {
             count = 0;
-            pc.printf("emg1 = %f, emg2 = %f\r\n", emg1.normalized, emg2.normalized);
+            //pc.printf("emg1 = %f, emg2 = %f\r\n", emg1.normalized, emg2.normalized);
             //pc.printf("Angle = %f, pidTerm = %f ,PID_scaled = %f, Error = %f, setpoint = %f\r\n", angle, pidTerm ,pidTerm_scaled, error1, setpoint);
             //pc.printf("Angle 2 = %f, pidTerm 2 = %f ,PID_scaled 2 = %f, Error 2 = %f, setpoint 2 = %f\r\n", angle2, pidTerm2 ,pidTerm_scaled2, error2,setpoint2);
         }