Diff: main.cpp

Revision:

33:d0621860756c

Parent:

32:2aaa9aae9dc2

--- a/main.cpp	Thu Oct 31 12:48:35 2019 +0000
+++ b/main.cpp	Thu Oct 31 15:02:44 2019 +0000
@@ -18,13 +18,16 @@
 #define PI 3.14159265
 
 Serial pc(USBTX, USBRX);            //connect to pc
-HIDScope scope(3);                  //HIDScope instance
+DigitalOut ledR(LED_RED);
+DigitalOut ledG(LED_GREEN);
+DigitalOut ledB(LED_BLUE);
+// HIDScope scope(3);                  //HIDScope instance
 DigitalOut motor0_direction(D7);    //rotation motor 1 on shield (always D6)
 FastPWM motor0_pwm(D6);             //pwm 1 on shield (always D7)
 DigitalOut motor1_direction(D4);    //rotation motor 2 on shield (always D4)
 FastPWM motor1_pwm(D5);             //pwm 2 on shield (always D5)
 Ticker loop_ticker;                 //used in main()
-Ticker scope_ticker;
+//Ticker scope_ticker;
 InterruptIn but1(SW2);              //button on mbed board
 InterruptIn but2(D9);               //debounced button on biorobotics shield
 InterruptIn but3(D8);               //button 1 on bio shield
@@ -131,6 +134,7 @@
     added to the EMG_params instance.
 */
 {
+    ledB = false;
     if (state_changed) {
         pc.printf("Started low value EMG calibration\r\nTime is %i\r\n", us_ticker_read());
         state_changed = false;
@@ -172,6 +176,8 @@
     added to the EMG_params instance.
 */
 {
+    ledB = true;
+    ledG = false;
     if (state_changed) {
         pc.printf("Started high value EMG calibration\r\nTime is %i\r\n", us_ticker_read());
         state_changed = false;
@@ -211,6 +217,8 @@
     rotating stage.
 */
 {
+    ledG = true;
+    ledR = false;
     if (state_changed) {
         pc.printf("Started encoder calibration\r\n");
         state_changed = false;
@@ -237,6 +245,18 @@
         pc.printf("Moving without magnet\r\n");
         state_changed = false;
     }
+    /*
+    //Boundaries erin knallen
+    float x = theta[1]*cos(theta[0]);
+    float y = theta[1]*sin(theta[0]);
+    
+    if (y <= 0.135 && (velocity_desired[1] < 0 || velocity_desired[0] < 0) && x <= 0.607){
+        velocity_desired[1] = 0;
+        velocity_desired[0] = 0;
+        pc.printf("X and Y inner boundary");
+    }   
+    */
+    
     theta_desired[0] = theta_desired[0] + dt*(velocity_desired[1]*cos(theta[0])/theta[1] - velocity_desired[0]*sin(theta[0])/theta[1]);
     theta_desired[1] = theta_desired[1] + dt*(velocity_desired[0]*cos(theta[0]) + velocity_desired[1]*sin(theta[0]));
     errors[0] = theta_desired[0] - theta[0];
@@ -390,9 +410,9 @@
     motor0_pwm.write(actuator.duty_cycle[0]);
     motor1_pwm.write(actuator.duty_cycle[1]);
     
-    scope.set(0, EMG_raw[0][0]-EMG_raw[0][1]);
-    scope.set(1, EMG_filtered[0]);
-    scope.set(2, (float)speed[0]/2.0f);
+    //scope.set(0, EMG_raw[0][0]-EMG_raw[0][1]);
+    //scope.set(1, EMG_filtered[0]);
+    //scope.set(2, (float)speed[0]/2.0f);
     /*
     scope.set(0, actuator.duty_cycle[1]);
     scope.set(1, theta[1]);
@@ -556,6 +576,10 @@
 
     theta_desired[0] = 0.0;
     theta_desired[1] = 0.63;
+    
+    ledR = true;
+    ledG = true;
+    ledB = true;
 
     actuator.magnet = false;
     EMG.max[0] = 0.01;
@@ -564,7 +588,7 @@
     but1.fall(&but1_interrupt);
     but2.fall(&but2_interrupt);
     but3.fall(&but3_interrupt);
-    scope_ticker.attach(&scope, &HIDScope::send, 0.05);
+    //scope_ticker.attach(&scope, &HIDScope::send, 0.05);
     loop_ticker.attach(&main_loop, dt); //main loop at 1kHz
     pc.printf("Main_loop is running\n\r");
     while (true) {