Diff: main.cpp

Revision:

2:fa90eaa14f99

Parent:

1:a9c933f1dc71

Child:

3:055eb9f256fc

--- a/main.cpp	Fri Oct 26 10:50:57 2018 +0000
+++ b/main.cpp	Fri Oct 26 10:59:24 2018 +0000
@@ -10,11 +10,6 @@
 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
-
 int sensor_sensitivity = 32;
 int gear_ratio = 131;
 float full_ratio  = gear_ratio*sensor_sensitivity*4;
@@ -24,10 +19,8 @@
 
 int counts_m1 = 0;
 int counts_m2 = 0;
-
 int counts_m1_prev = 0;
 int counts_m2_prev = 0;
-
 float deg_m1 = 0;
 float deg_m2 = 0;
 
@@ -39,7 +32,6 @@
 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)
 
-
 enum States {failure, waiting, calib_motor, calib_emg, operational, demo};
 enum Operations {rest, forward, backward, up, down};
 
@@ -53,11 +45,9 @@
 float thresholdtime = 1.0; // time waiting before switching modes
 
 Ticker      loop_timer;
-
 Ticker      sample_timer;
 Ticker      sample_timer2;
-
-HIDScope    scope( 5 );
+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
@@ -72,22 +62,20 @@
 volatile float filteredsignal2;//the first filtered emg signal 2
 
 bool state_changed = false;
-float high1;
-float abs1;
-float low1;
+
 void filterall()
 {
     //Highpass Biquad 5 Hz 
 static BiQuad HighPass1(0.95653708,  -1.91307417, 0.95653708, -1.91118480, 0.91496354);
-high1 = HighPass1.step(emg1_input);
+float 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)
-abs1 = fabs(high1);
+float abs1 = fabs(high1);
 float abs2 = fabs(high2);
     //Lowpass Biquad 10 Hz
 static BiQuad LowPass1(0.00362164,  0.00724327, 0.00362164, -1.82267251, 0.83715906);
-low1 = LowPass1.step(abs1);
+float low1 = LowPass1.step(abs1);
 static BiQuad LowPass2(0.00362164,  0.00724327, 0.00362164, -1.82267251, 0.83715906);
 float low2 = LowPass2.step(abs2);
 
@@ -117,15 +105,12 @@
             
 }
 
-
-
-
 void scopedata()
 {
     scope.set(0,emg1_input); // 
-    scope.set(1,high1); //    
-    scope.set(2,abs1); //
-    scope.set(3,low1);//
+    scope.set(1,emg1_input); //    
+    scope.set(2,emg1_input); //
+    scope.set(3,emg1_input);//
     scope.set(4,filteredsignal1);
     scope.send(); // send info to HIDScope server
 }
@@ -211,10 +196,7 @@
     
     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;
     }
     
@@ -347,19 +329,16 @@
 int main()
 {        
     motor1_speed_control.period_us(60); //60 microseconds PWM period, 16.7 kHz
-    
-    
-    
-    timer.start();
-    loop_timer.attach(&loop_function, Ts);    
-    sample_timer.attach(&scopedata, Ts);    
-    sample_timer2.attach(&filterall, Ts);
+           
+    //timer.start();
+    //loop_timer.attach(&loop_function, Ts);    
+    //sample_timer.attach(&scopedata, Ts);    
+    //sample_timer2.attach(&filterall, Ts);
 
-    
-    
-    
-     //led_red = 1;
-     //led_green = 1;
-    while (true) {              
+    while (true) { 
+     motor1_direction = 1; //set motor 1 to run counterclockwise (positive) direction for calibration
+    motor1_speed_control = 0.55;//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.55;             
     }
 }
\ No newline at end of file