Diff: main.cpp

Revision:

10:7339dca7d604

Parent:

5:0dd66c757f24

Child:

11:c8251a1362b7

Child:

13:3482d315877c

--- a/main.cpp	Mon Oct 22 19:13:37 2018 +0000
+++ b/main.cpp	Tue Oct 23 08:44:17 2018 +0000
@@ -10,9 +10,11 @@
 MODSERIAL pc(USBTX, USBRX);
 HIDScope    scope(2);
 
+// emg inputs
 AnalogIn    emg0( A0 );
 AnalogIn    emg1( A1 );
 
+// motor ouptuts
 PwmOut      motor1_pwm(D5);
 DigitalOut  motor1_dir(D4);
 PwmOut      motor2_pwm(D7);
@@ -33,6 +35,7 @@
 float e, u1, u2, emg_signal_raw_0, processed_emg_0, emg_signal_raw_1, processed_emg_1, robot_end_point, reference_end_point, motor_angle, motor_counts, q_ref; //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;
 
@@ -111,9 +114,10 @@
 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); //many different states can modify your reference position, so just do the inverse kinematics once, here
-        e = q_ref - motor_angle; //tracking error (q_ref - q_meas)
-        u1 = PID_controller(e); //feedback controller or with possibly fancy controller additions...;
+        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;
+        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.
 }
 
@@ -133,7 +137,8 @@
     motor2_pwm.period_us(60);
     current_state = waiting; //we start in state ‘waiting’ and current_state can be accessed by all functions
     u1 = 0.0f; //initial output to motors is 0.
-    loop_ticker.attach(&loop_function, 0.001f); //Run the function loop_function 1000 times per second
+    u2 = 0.0f;
+    loop_ticker.attach(&loop_function, 1/samplingfreq); //Run the function loop_function 1000 times per second
     
     while (true) { }  //Do nothing here (timing purposes)
 }