Diff: main.cpp

Revision:

50:2c03357de7cc

Parent:

49:818a0e90ed9c

Child:

51:b344a92b6a5f

--- a/main.cpp	Thu Nov 03 09:23:48 2016 +0000
+++ b/main.cpp	Thu Nov 03 09:55:11 2016 +0000
@@ -4,19 +4,22 @@
 #include "HIDScope.h"       //Hidscope by Tom Lankhorst
 #include "BiQuad.h"         //BiQuad by Tom Lankhorst
 #include "MODSERIAL.h"      //Modserial
+#include "QEI.h"            //QEI library for the encoders
 
 
 //=====================================================================================
 //Define objects
 
 //EMG
-AnalogIn    emg_biceps_right_in( A0 );              //analog in to get EMG biceps  (r) in to c++
+AnalogIn    emg_biceps_right_in (A0);               //analog in to get EMG biceps  (r) in to c++
 AnalogIn    emg_triceps_right_in(A1);               //analog in to get EMG triceps (r) in to c++
 AnalogIn    emg_biceps_left_in  (A2);               //analog in to get EMG biceps  (l) in to c++
 
 //Tickers
 Ticker      sample_timer;               //ticker for EMG signal sampling, analog becomes digital
 Ticker      ticker_switch;              //ticker for switch, every second it is possible to switch
+Ticker      ticker_referenceangle;      //ticker for the reference angle
+Ticker      ticker_controllerm1;        //ticker for the controller (PID) of motor 1
 
 //Monitoring
 HIDScope    scope(5);                   //open 5 channels in hidscope
@@ -25,18 +28,28 @@
 DigitalOut green(LED_GREEN);            //LED on K64f board, 1 is out; o is on
 
 //motors
-DigitalOut richting_motor1(D7); //motor 1 connected to motor 1 at k64f board; for turningtable
+DigitalOut richting_motor1(D7);         //motor 1 connected to motor 1 at k64f board; for turningtable
 PwmOut pwm_motor1(D6);
-DigitalOut richting_motor2(D4); ///motor 2 connected to motor 2 at k64f board; for linear actuator
+DigitalOut richting_motor2(D4);         //motor 2 connected to motor 2 at k64f board; for linear actuator
 PwmOut pwm_motor2(D5);
 
+//encoders
+DigitalIn encoder1A(D13);
+DigitalIn encoder1B(D12);
+DigitalIn encoder2A(D11);
+DigitalIn encoder2B(D10);
+
+//controller
+BiQuad PID_controller;
+
+
 //=====================================================================================
 //define variables
 
-//tresholds
-double treshold_biceps_right =0.04;                 //common values that work.
-double treshold_biceps_left=-0.04;                  // tested on multiple persons 
-double treshold_triceps=-0.04;                      //triceps and left biceps is specified negative, thus negative treshold
+//thresholds
+double treshold_biceps_right = 0.04;                 //common values that work.
+double treshold_biceps_left = -0.04;                  // tested on multiple persons 
+double treshold_triceps = -0.04;                      //triceps and left biceps is specified negative, thus negative treshold
 
 //on/off and switch signals
 int switch_signal = 0; //start of counter, switch made by even and odd numbers
@@ -49,7 +62,26 @@
 
 int cw=0;               //clockwise direction
 int ccw=1;              //counterclockwise direction
- 
+
+//encoder
+int counts_encoder1;
+//int counts_encoder2;
+float rev_counts_motor1;
+float rev_counts_motor1_rad;
+
+
+//reference
+volatile float d_ref = 0;
+const float w_ref = 3;
+const double Ts = 0.001;
+
+//controller
+const double Kp = 0.3823;
+const double Ki = 0.1279;
+const double Kd = 0.2519;
+const double N = 100;
+volatile double error1;
+volatile double controlOutput; 
 //=======================================
 //filter coefficients
 
@@ -158,6 +190,37 @@
         
         scope.send();                       //send all the signals to the scope
                 }
+                
+void reference(){
+    if (button_cw == 0){
+         d_ref = d_ref + w_ref * Ts;
+    }
+         if (d_ref > 21.36 ){
+            d_ref = 21.36;
+            //d_ref_const_cw = 1;
+        }
+    else{ 
+        d_ref = d_ref;
+    }
+    
+    if (button_ccw == 0){
+        d_ref = d_ref - w_ref * Ts;
+    }
+        if (d_ref < -21.36){
+        d_ref = -21.36;
+        
+    }
+    else{
+        d_ref = d_ref;
+    }
+    
+}
+
+void m1_controller(){
+    error1 = d_ref-rev_counts_motor1_rad;
+    ctrlOutput = PID_controller.step(error1);
+}
+
 //======================================================================   
 //program
 //======================================================================   
@@ -169,14 +232,18 @@
 red=0;           //led is on (0), at beginning
 
 //attach tickers to functions
-sample_timer.attach(&filter, 0.001);        //continously execute the EMG reader and filter, it ensures that filter and sampling is executed every 1/frequency seconds
+sample_timer.attach(&filter, Ts);        //continously execute the EMG reader and filter, it ensures that filter and sampling is executed every 1/frequency seconds
 ticker_switch.attach(&switch_function,1.0);
+ticker_referenceangle.attach(&reference, Ts);
+ticker_controllerm1.attach(&m1_controller, Ts);
+
+//PID controller
+PID_controller.PIDF(Kp,Ki,Kd,N,Ts);
 
 //Show the user what the starting motor will be and what will happen
 pc.printf("We will start the demonstration\r\n");
 pc.printf("\r\n\r\n\r\n");
 
-
     if (switch_signal%2==0)  
      {pc.printf("If you contract the biceps, the robot will go right \r\n");
      pc.printf("If you contract the triceps, the robot will go left \r\n");