Diff: main.cpp

Revision:

1:a9c933f1dc71

Parent:

0:3710031b2621

Child:

2:fa90eaa14f99

--- a/main.cpp	Fri Oct 19 10:37:25 2018 +0000
+++ b/main.cpp	Fri Oct 26 10:50:57 2018 +0000
@@ -1,60 +1,70 @@
 #include "mbed.h"
 #include "BiQuad.h"
 #include "HIDScope.h"
-#include <sys/time.h>
+#include <stdio.h>   
+#include <math.h>      
+#include <iostream>
+#include "QEI.h"
+
+
+Serial pc(USBTX,USBRX);
+Timer timer; 
+float Ts = 0.002;
+
+
+DigitalIn buttonR(D2);//rigth button on biorobotics shield
+DigitalIn buttonL(D3);//left button on biorobotics shield
 
-class Timer
-{
-private:
-    struct timeval start_t;
-public:
-    double start() { gettimeofday(&start_t, NULL); }
-    double get_ms() {
-       struct timeval now;
-       gettimeofday(&now, NULL);
-       return (now.tv_usec-start_t.tv_usec)/(double)1000.0 +
-              (now.tv_sec-start_t.tv_sec)*(double)1000.0;
-    }
-    double get_ms_reset() {
-      double res = get_ms();
-      reset();
-      return res;
-    }
-    Timer() { start(); }
-};
+int sensor_sensitivity = 32;
+int gear_ratio = 131;
+float full_ratio  = gear_ratio*sensor_sensitivity*4;
+
+QEI Encoder1(D10,D11,NC,sensor_sensitivity); //First one is B, Second one is A
+QEI Encoder2(D12,D13,NC,sensor_sensitivity); //
 
-Timer t();
-double used_ms;
-Serial pc(USBTX,USBRX);// serial connection to pc
+int counts_m1 = 0;
+int counts_m2 = 0;
+
+int counts_m1_prev = 0;
+int counts_m2_prev = 0;
 
-DigitalOut led_red(LED_RED);
-DigitalOut led_green(LED_RED);
-DigitalIn buttonR(D2);//rigth button on biorobotics shield
-DigitalIn buttonL(D3);//rigth button on biorobotics shield
+float deg_m1 = 0;
+float deg_m2 = 0;
+
+//Vector2d twist(0,0);
+//Matrix2f jacobian(0, 0, 0, 0), inv_jacobian(0, 0, 0, 0); 
 
 DigitalOut motor1_direction(D4);// draairichting motor 1 (1 is CW encoder als je daar op kijkt en CW shaft als je daar op kijkt)
 PwmOut motor1_speed_control(D5);//aanstuursnelheid motor 1
-
+PwmOut motor2_speed_control(D6);//aanstuursnelheid motor 2
 DigitalOut motor2_direction(D7);// draairichting motor 2 (1 is CW encoder als je daar op kijkt en CW shaft als je daar op kijkt)
-PwmOut motor2_speed_control(D6);//aanstuursnelheid motor 2
+
 
-enum States {failure, wait, calib_emg, operational, demo};
+enum States {failure, waiting, calib_motor, calib_emg, operational, demo};
 enum Operations {rest, forward, backward, up, down};
 
-States current_state;
+States current_state = calib_motor;
 Operations movement = rest;
 
-float max1 = 0.3; //initial threshold value for emg signals, changes during calibration
-float max2 = 0.3;
+float max1 = 0; //initial threshold value for emg signals, changes during calibration left arm
+float max2 = 0; // right arm
+float threshold1;
+float threshold2;
+float thresholdtime = 1.0; // time waiting before switching modes
+
+Ticker      loop_timer;
 
 Ticker      sample_timer;
-Ticker      loop_timer;
+Ticker      sample_timer2;
 
-HIDScope    scope( 3 );
+HIDScope    scope( 5 );
 
 AnalogIn raw_emg1_input(A0);//input for first emg signal 1, for the modes
 AnalogIn raw_emg2_input(A1);//input for first emg signal 2, for the strength
 
+volatile float emg1_input;
+volatile float emg2_input;
+
 volatile float raw_filteredsignal1;//the first filtered emg signal 1
 volatile float raw_filteredsignal2;//the first filtered emg signal 2
 
@@ -62,168 +72,294 @@
 volatile float filteredsignal2;//the first filtered emg signal 2
 
 bool state_changed = false;
-
-measureall(){ // changes all variables according in sync with the rest of the code
-    
-    emg1_input = emg1_input.read();
-    emg2_input = emg2_input.read();
-    filterall();
-    filteredsignal1 = raw_filteredsignal1;
-    filteredsignal2 = raw_filteredsignal2;
-    //Reading of motor
-    
-    
-    
-}
-
-
+float high1;
+float abs1;
+float low1;
 void filterall()
 {
     //Highpass Biquad 5 Hz 
-static BiQuad HighPass(0.95653708,  -1.91307417, 0.95653708, -1.91118480, 0.91496354);
-float high1 = HighPass.step(raw_emg1_input);
-float high2 = HighPass.step(raw_emg2_input);
+static BiQuad HighPass1(0.95653708,  -1.91307417, 0.95653708, -1.91118480, 0.91496354);
+high1 = HighPass1.step(emg1_input);
+static BiQuad HighPass2(0.95653708,  -1.91307417, 0.95653708, -1.91118480, 0.91496354);
+float high2 = HighPass2.step(emg2_input);
     // Rectify the signal(absolute value)
-float abs1 = fabs(high1);
+abs1 = fabs(high1);
 float abs2 = fabs(high2);
     //Lowpass Biquad 10 Hz
-static BiQuad LowPass(0.00362164,  0.00724327, 0.00362164, -1.82267251, 0.83715906);
-float low1 = LowPass.step(abs1);
-float low2 = LowPass.step(abs2);
+static BiQuad LowPass1(0.00362164,  0.00724327, 0.00362164, -1.82267251, 0.83715906);
+low1 = LowPass1.step(abs1);
+static BiQuad LowPass2(0.00362164,  0.00724327, 0.00362164, -1.82267251, 0.83715906);
+float low2 = LowPass2.step(abs2);
 
 raw_filteredsignal1 = low1;
 raw_filteredsignal2 = low2;
 
+
 }
 
+void measureall(){ // changes all variables according in sync with the rest of the code
+    
+    emg1_input = raw_emg1_input.read();
+    emg2_input = raw_emg2_input.read();
+    
+    filteredsignal1 = raw_filteredsignal1;
+    filteredsignal2 = raw_filteredsignal2;
+    
+    //Reading of motor
+            
+    counts_m1 = Encoder1.getPulses() - counts_m1_prev;
+    counts_m2 = Encoder1.getPulses() - counts_m2_prev;
+    deg_m1 = deg_m1 + counts_m1*(360/(full_ratio));
+    deg_m2 = deg_m2 + counts_m2*(360/(full_ratio));
+    counts_m1_prev = Encoder1.getPulses();
+    counts_m2_prev = Encoder2.getPulses();    
+            
+            
+}
+
+
+
+
 void scopedata()
 {
     scope.set(0,emg1_input); // 
-    scope.set(1,filteredsignal1); //    
-    scope.set(2,filteredsignal2); //
+    scope.set(1,high1); //    
+    scope.set(2,abs1); //
+    scope.set(3,low1);//
+    scope.set(4,filteredsignal1);
     scope.send(); // send info to HIDScope server
 }
 
-void loop_function() {
+
+ //////////////////// MOVEMENT STATES
+void do_forward(){
+    
+    //twist << 1, 0;
+    //Vector2d twistf(0,0);
+    //twistf << 1, 0;
+    if (filteredsignal2 > threshold2){
+        //double abs_sig = (filteredsignal2 - (0.5*max2))/(0.5*max2);
+        
+        //twist = twistf * abs_sig;
+        
+        }
+    
+    
+    if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
+        {
+            movement = backward;
+            timer.reset();
+            }
+    }
+
+void do_backward(){
+  
+    
+    
+     if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
+        {
+            movement = up;
+            timer.reset();
+            }
+    }
+
+void do_up(){
+    
+    //Code for moving up
+    
+     if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
+        {
+            movement = down;
+            timer.reset();
+            }
+    }
+void do_down(){
+    
+    //Code for moving down
+    
+     if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
+        {
+            movement = rest;
+            timer.reset();
+        
+            }
+    }
+void do_wait(){
+    
+    if ( filteredsignal2 > threshold2) {// 
+        current_state = waiting;
+        state_changed = true;
+    }              
+        if( timer.read() > thresholdtime && filteredsignal1 > threshold1)
+        {
+            movement = forward;
+            timer.reset();
+            }
+    }    
+///////////END MOVEMENT STATES/////////////////////////
+///////////ROBOT ARM STATES ///////////////////////////
+
+void do_state_failure(){    
+     
+    }
+    bool on = true;
+void do_state_calib_motor(){
+    if (state_changed==true) {
+        state_changed = false;        
+    }
+    
+    
+    if(on){
+        timer.reset();
+    motor1_direction = 1; //set motor 1 to run counterclockwise (positive) direction for calibration
+    motor1_speed_control = 0.3;//to make sure the motor will not run at too high velocity
+    motor2_direction = 0; // set motor 2 to run clockwise (negative) direction
+    motor2_speed_control = 0.3;
+    on = false;
+    }
+    
+    int deriv1 = fabs((counts_m1 - counts_m1_prev)/Ts);
+    int deriv2 = fabs((counts_m2 - counts_m2_prev)/Ts);
+    
+    
+    if ( timer.read() > 5 && deriv1 < 1 && deriv2 < 1) {
+        motor1_speed_control = 0;
+        motor2_speed_control = 0;
+        current_state = calib_emg;
+        timer.reset();
+        state_changed = true;
+    }
+    }
+void do_state_calib_emg(){    
+    if (state_changed==true) {
+        state_changed = false;        
+    }
+    
+    if(filteredsignal1 > max1){//calibrate to a new maximum
+       max1 = filteredsignal1;
+       threshold1 = 0.5*max1;
+       }
+    if(filteredsignal2 > max2){//calibrate to a new maximum
+       max2 = filteredsignal2;
+       threshold2 = 0.5 * max2;
+       }
+       
+    if (timer.read() > 10 && filteredsignal1 > threshold1 && filteredsignal2 > threshold2) {
+        current_state = operational;
+        timer.reset();
+        state_changed = true;
+    }    
+}
+
+void do_state_operational(){
+     if (state_changed==true) {
+        state_changed = false;        
+    }
+    
+    switch(movement) {// a separate function for what happens in each state
+    case rest:
+        do_wait();
+        break;
+    case forward:
+        do_forward();
+         
+        break;
+    case backward:
+        do_backward();
+        
+        break;    
+    case up:
+        do_up();        
+        break;
+    case down:
+        do_down();      
+        break;    
+    }
+    if (movement == rest && filteredsignal2 > threshold2) {
+        current_state = waiting;        
+        state_changed = true;
+    }       
+    
+}
+
+void do_state_waiting(){
+    if (state_changed==true) {
+        state_changed = false;        
+    }   
+       
+    if (filteredsignal1 > threshold1 && filteredsignal2 > threshold2) {
+        current_state = operational;
+        state_changed = true;
+    } 
+}
+//////////////// END ROBOT ARM STATES //////////////////////////////
+
+void motor_controller(){
+    float q1;
+    float q2;
+    //q1 defined
+    //q2 defined
+    
+    //float L0 = 0.1;
+    //float L1 = 0.1;
+    //float L2 = 0.4;
+    
+    //jacobian << L1, L2*sin(q1)+L1,-L0,-L0 - L2*cos(q1);
+        
+    //inv_jacobian = jacobian.inverse();
+    //Vector2d reference_vector = inv_jacobian*twist;
+    //float ref_v1 = reference_vector(0);
+    //float ref_v2 = reference_vector(1);
+    
+    //float ref_q1 = 0;
+    //ref_q1 = ref_p1 + 0.002*ref_v1;
+    // float ref_q2 = 0;
+    //ref_q2 = ref_p2 + 0.002*ref_v2;
+    
+    
+    
+    }
+
+void loop_function() { //Main loop function
     measureall();    
     switch(current_state) {
     case failure:
         do_state_failure(); // a separate function for what happens in each state
         break;
+    case calib_motor:
+        do_state_calib_motor();  
+        break ;
     case calib_emg:
         do_state_calib_emg();
         break;
     case operational:
         do_state_operational();
     break;
-    case wait;
-        do_state_wait();
+    case waiting:
+        do_state_waiting();
         break;  
 }
 motor_controller();
-scopedata(); // Outputs data to the computer
-}
-
-do_state_failure(){    
-    //al motor movement to zero!
-    wait(1000);    
-    };
-
-do_state_calib_emg(){    
-    if (state_changed==true) {
-        state_changed = false;        
-    }
-    if(filteredsignal1 > max1){//calibrate to a new maximum
-       max1 = filteredsignal1;
-       }
-    if(filteredsignal2 > max2){//calibrate to a new maximum
-       max2 = filteredsignal2;
-       }
-       
-    if (filteredsignal1 > (0.75*max1) && filteredsignal2 > (0.75*max2)) {
-        current_state = operational;
-        used_ms = t.get_ms_reset();
-        state_changed = true;
-    }    
-}
-
-do_state_operational(){
-     if (state_changed==true) {
-        state_changed = false;        
-    }
-    
-    switch(States) {// a separate function for what happens in each state
-    case rest:
-        if (filteredsignal2 > (0.6*max))) {// 
-        current_state = wait;
-        state_changed = true;
-    }              
-        if( t.get_ms() > 1000 && filteredsignal1 > (0.6max))
-        {
-            States = forward;
-            used_ms = t.get_ms_reset();
-            }
-        break;
-    case forward:
-        do_forward();
-         if( t.get_ms() > 1000 && filteredsignal1 > (0.6max))
-        {
-            States = backward;
-            used_ms = t.get_ms_reset();
-            }
-        break;
-    case backward:
-        do_backward();
-         if( t.get_ms() > 1000 && filteredsignal1 > (0.6max))
-        {
-            States = up;
-            used_ms = t.get_ms_reset();
-            }
-        break;    
-    case up:
-        do_up();
-         if( t.get_ms() > 1000 && filteredsignal1 > (0.6max))
-        {
-            States = wait;
-            used_ms = t.get_ms_reset();
-            }
-        break;
-    case down:
-        do_down();
-        if( t.get_ms() > 1000 && filteredsignal1 > (0.6max))
-        {
-            States = wait;
-            used_ms = t.get_ms_reset();
-            }
-        break;    
-    }
-       
-    
-}
-
-do_state_wait(){
-    if (state_changed==true) {
-        state_changed = false;        
-    }   
-       
-    if (filteredsignal1 > (0.75*max1) && filteredsignal2 > (0.75*max2) {
-        current_state = operational;
-        state_changed = true;
-    } 
+// Outputs data to the computer
 }
 
 
-
 int main()
-{      
-       
-    loop_timer.attach(&loop_function, 0.002);
+{        
+    motor1_speed_control.period_us(60); //60 microseconds PWM period, 16.7 kHz
+    
+    
     
-    pc.baud(115200);
+    timer.start();
+    loop_timer.attach(&loop_function, Ts);    
+    sample_timer.attach(&scopedata, Ts);    
+    sample_timer2.attach(&filterall, Ts);
+
     
-    while (true) {         
-    if(buttonR == true){
-        current_state = failure; 
-        }
+    
+    
+     //led_red = 1;
+     //led_green = 1;
+    while (true) {              
     }
 }
\ No newline at end of file