Diff: main.cpp

Revision:

2:625837aa7a56

Parent:

1:ffa6f4d78c8e

Child:

3:511a14a12629

--- a/main.cpp	Wed Oct 26 10:46:50 2016 +0000
+++ b/main.cpp	Thu Oct 27 07:54:43 2016 +0000
@@ -13,7 +13,7 @@
 DigitalOut motor2dir(D4);   //direction of motor 2, attach at m2, set to 0: ccw 
 
 Ticker control; //Ticker for processing encoder input
-volatile bool control_go=false;
+volatile bool controller_go=false;
 
 HIDScope scope(3); // aantal scopes in hidscope opzetten
 
@@ -32,22 +32,22 @@
 const double pi=3.14159265359;
 const double res = 64/(1/131.25*2*pi);     // resolution on gearbox shaft per pulse
 const double V_max=9.0;                    // maximum voltage supplied by trafo
-const double minRadius=0.0;                // minimum radius of arm
-const double maxRadius=0.3;                // maximum radius of arm
+const double minRadius=0.3;                // minimum radius of arm
+const double maxRadius=0.6;                // maximum radius of arm
 const double pulleyDiameter=0.0398;        // pulley diameter
 const double minAngle=-1.25;               // minimum angle for limiting controller
 
 QEI Encoder1(D13,D12,NC,64,QEI::X4_ENCODING); //defining encoder
 QEI Encoder2(D11,D10,NC,64,QEI::X4_ENCODING); //defining encoder
 
-
+double timer=0.001;  // needed for testing
 
 MODSERIAL pc(USBTX,USBRX);
 
-void activate_controller(){controller_go=true}; //activate go flag
+void activate_controller(){controller_go=true;} //activate go flag
 
 double PID( double err, const double Kp, const double Ki, const double Kd,
-const double Ts, const double N, double &v1, double &v2 ) {
+const double Ts, const double N, double &v1, double &v2 ) { //discrete PIDF filter
     const double a1 =-4/(N*Ts+2),
         a2=-(N*Ts-2)/(N*Ts+2),
         b0=(4*Kp + 4*Kd*N + 2*Ki*Ts+2*Kp*N*Ts+Ki*N*pow(Ts,2))/(2*N*Ts+4),
@@ -55,24 +55,28 @@
         b2=(4*Kp+4*Kd*N-2*Ki*Ts-2*Kp*N*Ts+Ki*N*pow(Ts,2))/(2*N*Ts+4);
 
     double v=err-a1*v1-a2*v2;
-    double u=b0*v+b1*v1+b2*v2
+    double u=b0*v+b1*v1+b2*v2;
     v2=v1; v1=v;
     return u;
 }
 
  
  void controller(){ //function for executing controller action              
-        //converting radius and theta to gearbox angle    
-        ref_angle1=16*theta;    
-        ref_angle2=-radius/pi/pulleyDiameter;
+        
+        double theta = sin(0.5*pi*timer); // just for testing
+        double radius = 0.3;              // just for testing
+        //converting radius and theta to gearbox angle 
+        double ref_angle1=16*theta;        
+        double ref_angle2=(-radius+minRadius)/pi/pulleyDiameter;
         
                 
-        angle1 = Encoder1.getPulses()/res;   //get number of pulses (counterclockwise is positive)
-        angle2 = Encoder2.getPulses()/res;   //get number of pulses
+        double angle1 = Encoder1.getPulses()/res;   //get number of pulses (counterclockwise is positive)
+        double angle2 = Encoder2.getPulses()/res;   //get number of pulses
         m1_pwm = (PID(ref_angle1-angle1,m1_Kp,m1_Ki,m1_Kd,m1_Ts,m1_N,m1_v1,m1_v2))/V_max; 
         //divide by voltage to get pwm duty cycle percentage)
         m2_pwm = (PID(ref_angle2-angle2,m2_Kp,m2_Ki,m2_Kd,m2_Ts,m2_N,m2_v1,m2_v2))/V_max;    
         
+        //limit pwm value and change motor direction when pwm becomes either negative or positive
         if (m1_pwm >=0.0f && m1_pwm <=1.0f){
             motor1dir=0;
             motor1.write(m1_pwm);
@@ -93,18 +97,15 @@
               
         //hidsopce to check what the code does exactly
         scope.set(0,angle1);
-        scope.set(1,refangle1-angle1);
-        scope.set(2,m1_pwm
+        scope.set(1,ref_angle1-angle1); //error
+        scope.set(2,m1_pwm);
         scope.send();
         
-        
+        timer=timer+0.001;
      }     
 
 
-        }
-
-int main()
-{
+int main(){
     pc.baud(115200);
     motor1.period(0.02f); //period of pwm signal for motor 1
     motor2.period(0.02f); // period of pwm signal for motor 2
@@ -113,8 +114,9 @@
     control.attach(&activate_controller,m1_Ts); //Ticker for processing encoder input
     pc.printf("RESET\n\r");
     while (true) {
-        if(controller_go){
+        if(controller_go){  // go flag
             controller_go=false;
             controller();
+        }
     }
 }
\ No newline at end of file