Diff: main.cpp

Revision:

30:ec66691d226d

Parent:

28:0460786f9573

Child:

31:98a1155f5edb

--- a/main.cpp	Mon Oct 19 00:16:39 2015 +0000
+++ b/main.cpp	Tue Oct 20 13:13:25 2015 +0000
@@ -65,10 +65,10 @@
 //PID variables
 double u1; double u2;                                               //Output of PID controller (PWM value for motor 1 and 2)
 double m1_error=0; double m1_e_int=0; double m1_e_prev=0;           //Error, integrated error, previous error
-const double m1_kp=1; const double m1_ki=0.0125; const double m1_kd=0.1;   //Proportional, integral and derivative gains.
+const double m1_kp=0.01; const double m1_ki=0.125; const double m1_kd=0.5;   //Proportional, integral and derivative gains.
 
 double m2_error=0; double m2_e_int=0; double m2_e_prev=0;           //Error, integrated error, previous error
-const double m2_kp=1; const double m2_ki=0.0125; const double m2_kd=0.1;   //Proportional, integral and derivative gains.
+const double m2_kp=0.01; const double m2_ki=0.125; const double m2_kd=0.5;   //Proportional, integral and derivative gains.
 
 
 //lowpass filter 7 Hz  - envelope
@@ -97,7 +97,7 @@
 
 //PID filter (lowpass ??? Hz)
 biquadFilter     derfilter( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 );   // derivative filter 
-
+biquadFilter     derfilter2( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); 
 /*--------------------------------------------------------------------------------------------------------------------
 ---- DECLARE FUNCTION NAMES ------------------------------------------------------------------------------------------
 --------------------------------------------------------------------------------------------------------------------*/
@@ -117,6 +117,7 @@
 
 //Reusable PID controller, previous and integral error need to be passed by reference
 double pid(double error, double kp, double ki, double kd,double &e_int, double &e_prev);
+double pid2(double error, double kp, double ki, double kd,double &e_int, double &e_prev);
 
 //Menu functions
 void mainMenu();
@@ -128,81 +129,89 @@
 ---- MAIN LOOP -------------------------------------------------------------------------------------------------------
 --------------------------------------------------------------------------------------------------------------------*/
 
+volatile bool looptimerflag;
+
+void setlooptimerflag(void)
+{
+    looptimerflag = true;
+} 
+
 int main()
 {
     pc.baud(115200);            //terminal baudrate
     red=1; green=1; blue=1;     //Make sure debug LEDS are off  
     
     //Set PwmOut frequency to 10k Hz???
-    //pwm_motor1.period(0.01);    
-    //pwm_motor2.period(0.01);
-    
-    dir_motor1.write(1);        //for motor 1 - 1 = counterclockwise
-    dir_motor2.write(1);        //for motor 2 - 1 = clockwise
-    pwm_motor1=0;
-    pwm_motor2=0;
+    pwm_motor1.period(0.0001);    
+    pwm_motor2.period(0.0001);
     
     
-    clearTerminal();            //Clear the putty window
+    //VARIABLES
+    AnalogIn potmeter(A4);
+    AnalogIn potmeter2(A5);
+    //DigitalIn button(D8);
+    //MODSERIAL pc(USBTX,USBRX);
     
-    // make a menu, user has to initiate next step
-    mainMenu();
-    //caliMenu();            // Menu function
-    //calibrate_arm();        //Start Calibration
+    //Encoder motor1(D13,D12);   // channel A and B from encoder, true - getSpeed works
+    //PwmOut pwm_motor1(D6);          // D4 and D5 = motor 2, D7 and D6 = motor 2  
+    //DigitalOut dir_motor1(D7);      // 
     
-    //calibrate_emg();        
-    wait(3);
-    start_control();        //100 Hz control
+    //Encoder motor2(D10,D9);   // channel A and B from encoder, true - getSpeed works
+   // PwmOut pwm_motor2(D5);          // D4 and D5 = motor 2, D7 and D6 = motor 2  
+   // DigitalOut dir_motor2(D4);      // 
     
-    //maybe some stop commands when a button is pressed: detach from timers.
-    //stop_control();
-    //stop_sampling();
-    char c;
-    pc.printf("entering while loop \r\n");
-    while(1) {
+    // MOTOR1
+    double goal;
+    double pwm_to_motor;  
+    // MOTOR2
+    double goal2;
+    double pwm_to_motor2;  
+        
+    //CODE
+    pc.baud(115200);
     
-    if( pc.readable() ){
-        c = pc.getc();
-        switch (c) 
-            {
-            case '1' :  
-                     x = x + 0.01; 
-                     //controlMenu();
-                     //running=false;
-                     break;
-          
-            case '2' :
-                     x-=0.01;
-                     //controlMenu();
-                     //running=false;
-                     break;
-                    
-            case '3' :
-                     y+=0.01;
-                     //controlMenu();
-                     //running=false;
-                     break;
-                     
-          
-            case '4' :
-                     y-=0.01;
-                     //controlMenu();                    
-                     //running=false;
-                     break;
-                     
-            case 'q' :
-                      pc.printf("Quit");
-                      //running = false;                    
-                      break;
-    }//end switch
-    pc.printf("Reference position: %f and %f \r\n",x,y);
-    pc.printf("Current position: %f and %f \r\n",current_x,current_y);
-    pc.printf("Current angles: %f and %f \r\n",theta1,theta2);
-    pc.printf("Error in angles: %f and %f \r\n",dtheta1,dtheta2);
-    pc.printf("PID output: %f and %f \r\n",u1,u2);
-    pc.printf("----------------------------------------\r\n\n");
-    }
-    //end if
+    //pwm_motor1.write(0.2f);         // Speed
+    //dir_motor1.write(1);            // Direction
+   
+    Ticker looptimer;
+    looptimer.attach(setlooptimerflag,0.01);  
+
+    while (1) {
+        
+        while(looptimerflag != true);
+        looptimerflag = false;
+        
+        //MOTOR1
+        goal = (potmeter.read()-0.5)*4200;
+        //pc.printf("setpoint: %f, %d, %f \n\r", goal, motor1.getPosition(),motor1.getSpeed());
+        double error1 = (goal - Encoder1.getPulses());
+        pwm_to_motor = pid(error1,m1_kp,m1_ki,m1_kd,m1_e_int,m1_e_prev); 
+    
+        if(pwm_to_motor > 0)
+            dir_motor1 = 1;     //=clockwise
+        else
+            dir_motor1 = 0;     //=counterclockwise
+        
+        pwm_motor1.write(abs(pwm_to_motor));
+        
+        //MOTOR2
+        goal2 = (potmeter2.read()-0.5)*4200;
+        
+        double error2=(goal2 - Encoder2.getPulses());
+        pwm_to_motor2 = pid2(error2,m2_kp,m2_ki,m2_kd,m2_e_int,m2_e_prev);
+    
+        if(pwm_to_motor2 > 0)
+            dir_motor2 = 0;         //=counterclockwise
+        else
+            dir_motor2 = 1;         //=clockwise
+        
+        pwm_motor2.write(abs(pwm_to_motor2));
+        //pc.printf("setpoint: %f, %d, %f \n\r", goal2, motor2.getPosition());   
+    
+       
+    pc.printf("goal: %f, pulses: %d \n\r", goal, Encoder1.getPulses());  
+    pc.printf("goal2: %f, pulses2: %d \n\r", goal2, Encoder2.getPulses());
+    
     }
     //end of while loop
 }
@@ -277,7 +286,20 @@
     // Integral
     e_int = e_int + CONTROL_RATE * error;
     // PID
-    return kp*error + ki*e_int + kd * e_der;
+    return kp*error + ki*e_int; //+ kd * e_der;
+    
+}
+
+double pid2(double error, double kp, double ki, double kd,double &e_int, double &e_prev)
+{
+    // Derivative
+    double e_der = (error-e_prev)/ CONTROL_RATE;
+    e_der = derfilter2.step(e_der);
+    e_prev = error;
+    // Integral
+    e_int = e_int + CONTROL_RATE * error;
+    // PID
+    return kp*error + ki*e_int; //+ kd * e_der;
     
 }