juijiu

Dependencies:   HIDScope QEI biquadFilter mbed

Fork of MotorArchitecture1-11 by Wouter Schuttert

Files at this revision

API Documentation at this revision

Comitter:
WouterJS
Date:
Fri Oct 26 10:50:57 2018 +0000
Parent:
0:3710031b2621
Child:
2:fa90eaa14f99
Commit message:
update!;

Changed in this revision

Eigen.lib Show annotated file Show diff for this revision Revisions of this file
QEI.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Eigen.lib	Fri Oct 26 10:50:57 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/ykuroda/code/Eigen/#13a5d365ba16
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Fri Oct 26 10:50:57 2018 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/aberk/code/QEI/#5c2ad81551aa
--- 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