Diff: main.cpp

Revision:

18:8002c75b8e20

Parent:

17:d1acb6888b82

Child:

19:fb3d570a115e

--- a/main.cpp	Fri Oct 18 13:05:44 2019 +0000
+++ b/main.cpp	Mon Oct 21 10:14:53 2019 +0000
@@ -6,6 +6,8 @@
 #include "FastPWM.h"
 #include <iostream>
 MODSERIAL pc(USBTX, USBRX);
+QEI motor2_pos (D8, D9, NC, 32);
+QEI motor1_pos (D12, D13, NC, 32);
 AnalogIn ain2(A2);
 AnalogIn ain1(A3);
 DigitalOut dir2(D4);
@@ -21,7 +23,7 @@
 Timeout EMG_peak;
 Timeout turn;
 Ticker      sample_timer;
-HIDScope    scope( 2);
+HIDScope    scope( 4);
 DigitalOut  ledred(LED_RED);
 DigitalOut  ledblue(LED_BLUE);
 DigitalOut  ledgreen(LED_GREEN);
@@ -38,7 +40,11 @@
 volatile bool ignore_turn=true;
 enum states{Waiting,Position_calibration,EMG_calibration,Homing,Operating,Demo,Failure};
 states currentState=Waiting;
-
+const float angle2_offset=asin(0.2);
+const float angle1_offset=asin(3.8/35.0);
+const double pi=3.1415926535897932384626;
+volatile float theta1;
+volatile float theta2;
 
 void read_emg()
 {
@@ -80,18 +86,70 @@
     RMS1[count1%150]=If1;
     /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
     P1=sqrt(RMS_value1);
-    scope.set(0, P0 ); // send root mean squared
-    scope.set(1, notched0);
     /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) 
     *  Ensure that enough channels are available (HIDScope scope( 2 ))
     *  Finally, send all channels to the PC at once */
-    scope.send();
     /* To indicate that the function is working, the LED is toggled */
     ledred=1;
     ledgreen=0;
     ledblue=1;
 }
 
+void get_angles(void)
+{
+    float pulses1=motor1_pos.getPulses();
+    float pulses2=motor2_pos.getPulses();
+    theta1=angle1_offset+pulses1*16.0/17.0*2*pi/131.0/32.0;
+    theta2=angle2_offset+pulses2*16.0/17.0*2*pi/131.0/32.0; 
+}
+
+void pos_cal(void)
+{
+    float t=state_time.read();
+    static int pos_time_counter=0;
+    static int last_ticks=10000;
+    static bool motor1_calibrated=false;
+    float pulses;
+    pos_time_counter+=1;
+    if(!motor1_calibrated&&t>1.0f)
+    {
+        dir1=1; //???
+        motor1_pwm.write(0.6f);
+        pulses=motor1_pos.getPulses();
+        if(pos_time_counter%500==0&&fabs(pulses-last_ticks)<1)
+        {
+            motor1_pos.reset();
+            motor1_calibrated=true;
+            last_ticks=10000;
+            state_time.reset();
+            dir1=!dir1;
+        }
+        else if(pos_time_counter%500==0)
+        {
+            last_ticks=motor1_pos.getPulses();
+        }
+        
+    }
+    else if(t>1.0f)
+    {
+        motor1_pwm.write(0.0f);
+        dir2=1; //???
+        motor2_pwm.write(0.6f);
+        pulses=motor2_pos.getPulses();
+        if(pos_time_counter%500==0&&fabs(pulses-last_ticks)<1)
+        {
+            motor2_pos.reset();
+            motor2_pwm.write(0.0f);
+            currentState=EMG_calibration;
+        }
+        else if(pos_time_counter%500==0)
+        {
+            last_ticks=motor2_pos.getPulses();
+        }
+    }   
+    
+}
+
 void record_min_max(void)
 {
     float t=state_time.read();
@@ -164,6 +222,12 @@
 
 void sample()
 {
+    get_angles();
+    scope.set(0,P0);
+    scope.set(1,P1);
+    scope.set(2,theta1);
+    scope.set(3,theta2);
+    scope.send(); 
     switch(currentState)
     {
         case Waiting:
@@ -175,6 +239,7 @@
             ledred=1;
             ledgreen=1;
             ledblue=0;
+            pos_cal();
             break;
         case EMG_calibration:
             ledred=1;
@@ -215,10 +280,10 @@
     currentState=Failure;
 }
 
-void button_press() 
+void button_press(void) 
 //Press button to change state
 {
-    state_time.start();
+    state_time.reset();
     switch(currentState)
     {
         case Waiting:
@@ -259,7 +324,7 @@
     int frequency_pwm=10000;
     motor1_pwm.period(1.0/frequency_pwm);
     motor2_pwm.period(1.0/frequency_pwm);
-    
+    state_time.start();
     while (true) {
         wait(10);
     }