Diff: main.cpp

Branch:

bla

Revision:

14:4744cc6c90f4

Parent:

12:3c47c7b1d1d7

Child:

15:7d3b138446fa

--- a/main.cpp	Thu Oct 25 07:59:00 2018 +0000
+++ b/main.cpp	Thu Oct 25 08:14:51 2018 +0000
@@ -20,10 +20,15 @@
 PwmOut      motor2_pwm(D7);
 DigitalOut  motor2_dir(D6);
 
+// defining encoders
+//QEI motor_1_encoder(D12,D13,32);
+//QEI motor_2_encoder(D12,D13,32);
+
 AnalogIn    potmeter1(A2);
 AnalogIn    potmeter2(A3);
 DigitalIn   button(D0);
 
+// tickers and timers
 Ticker Sample;
 Timer state_timer;
 
@@ -32,7 +37,7 @@
 //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_counts, q_ref; //will be set by the motor_controller function
+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
 int counts_per_rotation = 32;
 bool state_changed = false;
 double samplingfreq = 1000;
@@ -46,8 +51,11 @@
 
 void measure_all() 
 {
-    motor_angle = motor_counts*2.0f*3.1415926535f/counts_per_rotation; //do this here, and not in the encoder interrupt, to reduce computational load
-    robot_end_point = forwardkinematics_function(motor_angle);  //motor_angle is global, this function ne
+    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;
+    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();
     processed_emg_0 = processing_chain_emg(0);  // some function ‘float my_emg_processing_chain()’ that returns a float. The raw emg is global