Diff: main.cpp

Revision:

34:6e74f6629a0e

Parent:

33:97e69c32a768

Child:

35:82b432f5aef7

--- a/main.cpp	Fri Nov 03 00:25:57 2017 +0000
+++ b/main.cpp	Fri Nov 03 01:21:32 2017 +0000
@@ -23,6 +23,7 @@
     Ticker      max_read1;
     Ticker      max_read3;
     Ticker      Motorcontrol;
+    //Ticker      PIDtimer;
     HIDScope    scope( 4 );
     DigitalOut  red(LED_RED);
     DigitalOut  blue(LED_BLUE);
@@ -103,17 +104,17 @@
     // This value also adds a very slight gain to every value
 
 // RKI Variables & Constants
-    float setpoint1;
-    float setpoint2;
+    double setpoint1 = 2.35;
+    double setpoint2 = 0.785;
+    double x = 10.77;
+    double y = 15.73;
     float pi = 3.14159265359;
     float a = 22.25;             //originally 22.25, makes for xinitial=10.766874   // length of arm 1
     float b = 26.5;             //originally 16.48   makes for yinitial=15.733      // length of arm 2
-    float alpha_old = 2.35;     //INITIAL ANGLES IN RADIANS
-    float beta_old = 0.785;
+    double alpha_old = 2.35;     //INITIAL ANGLES IN RADIANS
+    double beta_old = 0.785;
     float delta1;
     float delta2;
-    float x = 10.77;
-    float y = 15.73;
     float diffmotor_a;
     float diffmotor_b;
 
@@ -369,7 +370,7 @@
 // EMG Filtering & Scope
 //
 //
-void filter() {
+void filter(/*double setpoint1, double setpoint2*/) {
     // Right Biceps
     emg1 = emg1_in.read();
     emg1highfilter = filterhigh1.step(emg1);
@@ -515,11 +516,12 @@
             green = 0;
             //MV1 = 0.5;
             //MV2 = 0;
-            x = x + 0.5;
-            float alpha = inversekinematics1(x,y); //calculate alpha and beta for current x,y
-            float beta = inversekinematics2(x,y);
+            x = x + 0.2;
+            double alpha = inversekinematics1(x,y); //calculate alpha and beta for current x,y
+            double beta = inversekinematics2(x,y);
             setpoint1 = alpha;
             setpoint2 = beta;
+            wait(0.2);
         }            
         // This part checks for left biceps contractions only  
         else if (maxpart1>MOVAVG_RB && maxpart2<MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){
@@ -528,11 +530,12 @@
             green = 1;
             //MV1 = -0.5;
             //MV2 = 0;
-            x = x - 0.5;
-            float alpha = inversekinematics1(x,y);
-            float beta = inversekinematics2(x,y);
+            x = x - 0.2;
+            double alpha = inversekinematics1(x,y);
+            double beta = inversekinematics2(x,y);
             setpoint1 = alpha;
             setpoint2 = beta;
+            wait(0.2);
         }
         // This part checks for left lower arm contractions only  
         else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4>MOVAVG_RL){
@@ -541,11 +544,12 @@
             green = 1;
             //MV1 = 0;
             //MV2 = 0.5;
-            y = y + 0.5;
-            float alpha = inversekinematics1(x,y);
-            float beta = inversekinematics2(x,y);
+            y = y + 0.2;
+            double alpha = inversekinematics1(x,y);
+            double beta = inversekinematics2(x,y);
             setpoint1 = alpha;
             setpoint2 = beta;
+            wait(0.2);
         }
         // This part checks for right lower arm contractions only
         else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4<MOVAVG_RL){
@@ -554,11 +558,12 @@
             green = 0;
             //MV1 = 0;
             //MV2 = -0.5;
-            y = y - 0.5;
-            float alpha = inversekinematics1(x,y);
-            float beta = inversekinematics2(x,y);
+            y = y - 0.2;
+            double alpha = inversekinematics1(x,y);
+            double beta = inversekinematics2(x,y);
             setpoint1 = alpha;
             setpoint2 = beta;
+            wait(0.2);
         }           
 /*        // This part checks for both lower arm contractions only 
         else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4<MOVAVG_RL){
@@ -609,15 +614,16 @@
     scope.send(); // send everything to the HID scope 
     
 }
-
+/*
 // PID calculations
 //
 //
-void PIDcalculation() {
+void PIDcalculation1() {
 //PID calculation for motor 1
+    filter();
     count1 = Encoder1.getPulses();
-    angle1 += (0.0981 * count1);
-    new_error1 = setpoint1 - alpha_old;
+    angle1 += (0.000748 * count1);
+    new_error1 = setpoint1 - angle1;
     
     change_error1 = new_error1 - last_error1;
     total_error1 += new_error1;
@@ -631,11 +637,13 @@
     pid_term_scaled1 = abs(pid_term1);
     
     last_error1 = new_error1;
-    
+}
+void PIDcalculation2() {    
 //PID calculation for motor 2
+    filter();
     count2 = Encoder2.getPulses();
-    angle2 += (0.0981 * count2);
-    new_error2 = setpoint2 - beta_old;
+    angle2 += (0.000748 * count2);
+    new_error2 = setpoint2 - angle2;
     
     change_error2 = new_error2 - last_error2;
     total_error2 += new_error2;
@@ -650,29 +658,33 @@
     
     last_error2 = new_error2;
 }
-   
+*/  
 // Motor control
 //
 //
 // check to see if motor1 needs to be activated
-void SetMotor1(float angle1, float setpoint1) {
-    PIDcalculation();
+void SetMotor1() {
+    //PIDcalculation1();
+    filter();
+    angle1 += (0.0981 * count1);
     if (angle1<setpoint1){
     motor1direction = 0; // counterclockwise rotation
     }
     else {
     motor1direction = 1; // clockwise rotation
     }
-    if (angle2<setpoint2 || angle2>setpoint2) {
-    motor2pwm = 0.5;
+    if (angle1<setpoint1 || angle1>setpoint1) {
+    motor1pwm = 0.5;
     }
-    else if (angle2 == setpoint2){
-    motor2pwm = 0;
+    else if (angle1 == setpoint1){
+    motor1pwm = 0;
     }
 }
 // check if motor1 needs to be activated
-void SetMotor2(float angle2, float setpoint2) {
-    PIDcalculation();
+void SetMotor2() {
+    filter();
+    //PIDcalculation2();
+    angle2 += (0.0981 * count2);
     if (angle2<setpoint2){
     motor2direction = 0; // counterclockwise rotation
     }
@@ -688,12 +700,8 @@
 }
 
 void MeasureAndControl(void) {
-    
-    // This function measures the potmeter position, extracts a
-    // reference velocity from it, and controls the motor with 
-    // a simple FeedForward controller. Call this from a Ticker.
-    SetMotor1(angle1,setpoint1);
-    SetMotor2(angle2,setpoint2);
+    SetMotor1();
+    SetMotor2();
 }
 
     
@@ -703,5 +711,7 @@
     max_read1.attach(&get_max1, 2); // set the frequency of the calibration loop at 0.5Hz
     max_read3.attach(&get_max3, 2);
     Motorcontrol.attach(&MeasureAndControl,0.5);
+    //PIDtimer.attach(&PIDcalculation1, 0.005);
+    //PIDtimer.attach(&PIDcalculation2, 0.005);
     while(1) {}
 }
\ No newline at end of file