Diff: main.cpp

Branch:

bla

Revision:

16:0280a604cf7e

Parent:

15:7d3b138446fa

Child:

17:1f93c83e211f

--- a/main.cpp	Thu Oct 25 08:51:32 2018 +0000
+++ b/main.cpp	Thu Oct 25 17:50:26 2018 +0000
@@ -21,28 +21,25 @@
 DigitalOut  motor2_dir(D6);
 
 // defining encoders
-QEI motor_1_encoder(D12,D13,32);
-QEI motor_2_encoder(D10,D11,32);
+QEI motor_1_encoder(D12,D13,NC,32);
+QEI motor_2_encoder(D10,D11,NC,32);
 
 AnalogIn    potmeter1(A2);
 AnalogIn    potmeter2(A3);
 DigitalIn   button(D0);
 
 // tickers and timers
-Ticker Sample;
+Ticker loop_ticker;
 Timer state_timer;
 
 enum States {failure, waiting, calib_enc, calib_emg, operational, demo}; //All possible robot states
 
 //Global variables/objects
 States current_state;
-Ticker loop_ticker; //The Ticker object that will ensure perfect timing of our looping code
-double x,y, qref1, qref2 ,e, e1, e2, u1, u2, emg_signal_raw_0, processed_emg_0, emg_signal_raw_1, processed_emg_1, robot_end_point, reference_end_point, motor_angle, q1, q2, motor_1_counts, motor_2_counts; //will be set by the motor_controller function
+
+double des_vx, des_vy, x, y, q1, q2, qref1, qref2, e1, e2, u1, u2, emg_raw_0, processed_emg_0, emg_raw_1, processed_emg_1; //will be set by the motor_controller function
 int counts_per_rotation = 32;
 bool state_changed = false;
-double samplingfreq = 1000;
-double x; // Making the position (x,y) of the end effector global
-double y;
 const double T = 0.001;
 
 float processing_chain_emg(int num) {
@@ -51,22 +48,20 @@
 
 void measure_all() 
 {
-    motor_1_counts = motor_1_encoder.getPulses();
-    motor_2_counts = motor_2_encoder.getPulses();
-    q1 = motor_1_counts*2.0f*3.1415926535f/counts_per_rotation; //do this here, and not in the encoder interrupt, to reduce computational load
-    q2 = motor_2_counts*2.0f*3.1415926535f/counts_per_rotation;
+    q1 = motor_1_encoder.getPulses()*2.0f*3.1415926535f/counts_per_rotation; //do this here, and not in the encoder interrupt, to reduce computational load
+    q2 = motor_2_encoder.getPulses()*2.0f*3.1415926535f/counts_per_rotation;
     forwardkinematics_function(q1,q2,x,y);  //motor_angle is global, this function ne
-    emg_signal_raw_0 = emg0.read(); //sample analog voltages (all sampling theory applies, you might get aliasing etc.)
-    emg_signal_raw_1 = emg1.read();
+    emg_raw_0 = emg0.read(); //sample analog voltages (all sampling theory applies, you might get aliasing etc.)
+    emg_raw_1 = emg1.read();
     processed_emg_0 = processing_chain_emg(0);  // some function ‘float my_emg_processing_chain()’ that returns a float. The raw emg is global
     processed_emg_1 = processing_chain_emg(1);
 }
 
 void output_all() {
     motor1_pwm = fabs(u1);
-    motor1_dir = u1 > 0.5f;
+    motor1_dir = u1 > 0.0f;
     motor2_pwm = fabs(u2);
-    motor2_dir = u2 > 0.5f;
+    motor2_dir = u2 > 0.0f;
     static int output_counter = 0;
     output_counter++;
     if (output_counter == 100) {pc.printf("Something something... %f",u1); output_counter = 0;}  //Print to screen at 10 Hz with MODSERIAL
@@ -102,9 +97,8 @@
             break;
         
         case operational:       //interpreting emg-signals to move the end effector
-            if (state_changed==true) { int x = 5; }
+            if (state_changed==true) { ; }
             // example
-            reference_end_point = robot_end_point + processed_emg_0;
             if (button.read() == true) { current_state = demo; }
             
             break;
@@ -124,9 +118,9 @@
 void motor_controller() 
 {
     if (current_state >= operational) { // we can (ab)use the fact that an enum is actually an integer, so math/logic rules still apply
-        q_ref += inversekinematics_function(reference_end_point)/samplingfreq; //many different states can modify your reference position, so just do the inverse kinematics once, here
-        e1 = q_ref - motor_angle; //tracking error (q_ref - q_meas)
-        e2 = q_ref - motor_angle;
+        inversekinematics_function(x,y,T,qref1,qref2,q1,q2,des_vx,des_vy); //many different states can modify your reference position, so just do the inverse kinematics once, here
+        e1 = qref1 - q1; //tracking error (q_ref - q_meas)
+        e2 = qref2 - q2;
         PID_controller(e1,e2,u1,u2); //feedback controller or with possibly fancy controller additions...; pass by reference
         } //otherwise we just don’t mess with the value of control variable ‘u’ that is set somewhere in the state-machine.
 }