Patrick Zieverink / Mbed 2 deprecated biorobotics_four_scorers_29-10

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

Files at this revision

API Documentation at this revision

Revision 29:fa864b0f62d8, committed 2019-10-29

Comitter:
PatrickZieverink
Date:
Tue Oct 29 11:49:44 2019 +0000
Parent:
28:f08665a5ef6c
Commit message:
Not working version;

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file 
--- a/main.cpp	Mon Oct 28 16:01:30 2019 +0000
+++ b/main.cpp	Tue Oct 29 11:49:44 2019 +0000
@@ -18,7 +18,7 @@
 #define PI 3.14159265
 
 Serial pc(USBTX, USBRX);            //connect to pc
-HIDScope scope(4);                  //HIDScope instance
+HIDScope scope(5);                  //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)
@@ -32,6 +32,10 @@
 AnalogIn EMG1_sig(A2);
 AnalogIn EMG1_ref(A3);
 
+//Joystick test
+AnalogIn vrx(A4);                   //Joystick_x
+AnalogIn vry(A5);                   //Joystick_y
+
 void check_failure();
 int schmitt_trigger(float i);
 
@@ -48,7 +52,7 @@
 States state;                       //using the States enum
 struct actuator_state {
     float duty_cycle[2];            //pwm of 1st motor
-    int direction[2];               //direction of 1st motor
+    bool direction[2];               //direction of 1st motor
     int default_direction[2];
     bool magnet;                    //state of the magnet
 } actuator;
@@ -192,14 +196,15 @@
         pc.printf("Moving without magnet\r\n");
         state_changed = false;
     }
-    theta_desired[0] = theta[0] + dt*(velocity_desired[1]*cos(theta[0])/theta[1] - velocity_desired[0]*sin(theta[0])/theta[1]);
-    theta_desired[1] = theta[1] + dt*(velocity_desired[0]*cos(theta[0]) - velocity_desired[1]*sin(theta[0]));
+    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];
     errors[1] = theta_desired[1] - theta[1];
     if (button2_pressed) {
         pc.printf("positions: (rad, m): %f %f\r\n"
                   "Errors:              %f %f\r\n"
-                  "Previous actions:    %f %f\r\n", theta[0], theta[1], errors[0], errors[1], actuator.duty_cycle[0], actuator.duty_cycle[1]);
+                  "Previous actions:    %f %f\r\n"
+                  "Vx, Vy:              %f %f\r\n", theta[0], theta[1], errors[0], errors[1], actuator.duty_cycle[0], actuator.duty_cycle[1], velocity_desired[0],velocity_desired[1]);
         button2_pressed = false;
     }
 }
@@ -214,8 +219,8 @@
         pc.printf("Moving with magnet\r\n");
         state_changed = false;
     }
-    theta_desired[0] = theta[0] + dt*(velocity_desired[1]*cos(theta[0])/theta[1] - velocity_desired[0]*sin(theta[0])/theta[1]);
-    theta_desired[1] = theta[1] + dt*(velocity_desired[0]*cos(theta[0]) - velocity_desired[1]*sin(theta[0]));
+    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];
     errors[1] = theta_desired[1] - theta[0];
 }
@@ -228,8 +233,8 @@
         pc.printf("Started homing\r\n");
         state_changed = false;
     }
-    theta_desired[0] = theta[0] + dt*(velocity_desired[1]*cos(theta[0])/theta[1] - velocity_desired[0]*sin(theta[0])/theta[1]);
-    theta_desired[1] = theta[1] + dt*(velocity_desired[0]*cos(theta[0]) - velocity_desired[1]*sin(theta[0]));
+    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];
     errors[1] = theta_desired[1] - theta[0];
 }
@@ -258,9 +263,9 @@
             EMG_filtered[c] = filter_value[c];
         }
         enc_value[c] -= enc_zero[c];
-        theta[c] = (float)(enc_value[c])/(float)(8400*2*PI);
+        theta[c] = (float)(enc_value[c])/(float)(8400*2*PI);    //Revoluties        Theta 0 en 1 zijn de gemeten waardes hier!
     }
-    theta[1] = theta[1]*(5.05*0.008*2*PI)+0.63;
+    theta[1] = theta[1]*(5.05*0.008*2*PI)+0.63;                 
     
     for(int c = 0; c<2; c++) {
         speed[c] = schmitt_trigger(EMG_filtered[c]);
@@ -277,7 +282,24 @@
         if (speed[c] == 2){
             velocity_desired[c] = 0.02f;
         }
+        
+        // Joystick beweging
+        if ( c == 0){
+            velocity_desired[c] = (vrx.read()*100-50)/50*0.02;
+            if (velocity_desired[c] < 0.002 && velocity_desired[c] > -0.002){
+                velocity_desired[c] = 0;
+            }
+            //pc.printf("v1 = %f | ", velocity_desired[c]);
+        }
+        if ( c == 1){
+            velocity_desired[c] = (vry.read()*100-50)/50*0.02;
+            if (velocity_desired[c] < 0.002 && velocity_desired[c] > -0.002){
+                velocity_desired[c] = 0;
+            }
+            //pc.printf("v2 = %f | ", velocity_desired[c]);
+        }
     }
+    //pc.printf(" theta1 = %f | theta2 = %f | theta1des = %f | theta2des = %f | error1 = %f | error2 = %f \r\n", theta[0], theta[1], theta_desired[0], theta_desired[1], errors[0], errors[1]);
 }
 
 void motor_controller() { //s_idle, s_cali_EMG, s_cali_enc, s_moving_magnet_off, s_moving_magnet_on, s_homing, s_failure
@@ -309,15 +331,32 @@
 }
 
 void output()
-{
+{   
+    for (int c = 0; c <2; c++) {
+        if (actuator.default_direction[c] == false){
+            if (actuator.direction[c] == 1){
+                actuator.direction[c] = 0;
+            }
+            else {
+                actuator.direction[c] = 1;
+            }
+        }
+    }
+    
     motor0_direction = actuator.direction[0]*actuator.default_direction[0];
     motor1_direction = actuator.direction[1]*actuator.default_direction[1];
     motor0_pwm.write(actuator.duty_cycle[0]);
     motor1_pwm.write(actuator.duty_cycle[1]);
-    
+    /*
     scope.set(0, EMG_filtered[0]);
     scope.set(1, speed[0]);
     scope.set(2, actuator.duty_cycle[0]);
+    */
+    scope.set(0, actuator.duty_cycle[1]);
+    scope.set(1, motor1_direction);
+    scope.set(2, actuator.duty_cycle[1]);
+    scope.set(3, theta[1]);
+    scope.set(4, theta_desired[1]);
 }
 
 void state_machine()
@@ -399,7 +438,7 @@
 {
     pc.baud(115200);
     pc.printf("Executing main()... \r\n");
-    state = s_idle;
+    state = s_idle;                // Hij slaat nu dus de calibratie over!
 
     motor0_pwm.period(1.0f/160000.0f);   // 1/frequency van waarop hij draait
     motor1_pwm.period(1.0f/160000.0f);   // 1/frequency van waarop hij draait
@@ -407,10 +446,10 @@
     actuator.direction[0] = 0;
     actuator.direction[1] = 0;
 
-    actuator.default_direction[0] = -1;
-    actuator.default_direction[1] = 1;
+    actuator.default_direction[0] = true;
+    actuator.default_direction[1] = false;
     
-    PID.P[0] = 1.0;
+    PID.P[0] = 0.01;
     PID.P[1] = 1.0;
     PID.I[0] = 0.0;
     PID.I[1] = 0.0;
@@ -418,6 +457,9 @@
     PID.D[1] = 0.0;
     PID.I_counter[0] = 0.0;
     PID.I_counter[1] = 0.0;
+    
+    theta_desired[0] = 0.0;
+    theta_desired[1] = 0.63;
         
     actuator.magnet = false;
     EMG.max[0] = 0.01;