Diff: main.cpp

Revision:

4:f6862a157db9

Parent:

3:750afb8580dd

Child:

5:3eace29679bc

--- a/main.cpp	Mon Oct 21 12:17:59 2019 +0000
+++ b/main.cpp	Wed Oct 23 14:47:48 2019 +0000
@@ -9,13 +9,15 @@
 
 
 Serial pc(USBTX, USBRX);
-volatile float width_percent;
+DigitalIn but_1(D1); //D1 to BUT1
+DigitalOut msignal_1(D6); //Signal to motor controller 
+DigitalOut direction_1(D7); //ouput pin for direction of rotation
 
-const float k_p = 3;  
-const float k_i = 1000;
-const float k_d = 1;
-const float t_s = 0.0025; //sample time in sec
-
+//These seemed the best values after trying them out and using wikipedia's info
+const float k_p_1 = 1;  
+const float k_i_1 = 1;
+const float k_d_1 = 0.1;
+const float t_s = 0.0025; //sample time in sec; same for both motors
 
 //inputs for LPF; still to fill in!
 float b1;
@@ -24,64 +26,106 @@
 float b4;
 float b5;
 
-
+float outcome_1 = 0;
+float setpoint_1;
+bool dir_1;
+bool moving_1 = false;
 
-float getError(){
+float getSetpoint_1(){
     float a; //error
     cin >> a;   //input on keyboard
     return a;
     }
     
+float getError_1(){
+    float errorSum_1 = setpoint_1 - outcome_1;
+    return errorSum_1;
+    }
+
+float checkMovement_1(){
+    if (outcome_1 >= 1){
+        dir_1 = true;   //CCW
+        moving_1 = true;
+        pc.printf("Positive movement\r\n");
+        }
+    else if (outcome_1 <= -1){
+        dir_1 = false;     //CW
+        moving_1 = true;
+        pc.printf("Negative movement\r\n");
+        }
+     else{
+        direction_1 = 0;
+        moving_1 = false;
+        pc.printf("No movement\r\n");
+        }
+    }
     
+    void motor_1 (){ //runs the motor
+  
+    if(moving_1){
+    msignal_1 = 1; // Turn motor on
+    }
+    else{
+    msignal_1 = 0; // Turn motor off
+    }
+    
+ }
+    
+
 /*PID controller code
 Proportional part general formula: u_k = k_p * e 
 Integral part general formula: u_i = k_i * integral e dt
 Derivative part general formula: u_d = k_d * derivative e */
 
-
-
-float PIDControl(float error){
+float PIDControl_1(float error){
     static float error_integral = 0;
     static float error_prev = error;
     static BiQuad LPF (b1, b2, b3, b4, b5); 
     
     //proportional
-    float u_k = k_p * error;
+    float u_k_1 = k_p_1 * error;
     
     //integral
     error_integral = error_integral + error * t_s;
-    float u_i = k_i * error_integral;
+    float u_i_1 = k_i_1 * error_integral;
     
     //differential
     float error_derivative = (error - error_prev) / t_s;
    // float filtered_error = LPF.step(error_derivative); //LPF with function of BiQuad library
-   // float u_d = k_d * filtered_error;
-    float u_d = k_d * error_derivative; //this is very sensitive to noise, hence the LPF above
+   // float u_d_1 = k_d_1 * filtered_error;
+    float u_d_1 = k_d_1 * error_derivative; //this is very sensitive to noise, hence the LPF above
     error_prev = error;
     
-    return u_k + u_i + u_d;
+    return u_k_1 + u_i_1 + u_d_1;
     }
     
-
-int main()
-{
+    
+int main(){   
     pc.baud(115200);
+    startmain:
     pc.printf("--------\r\nWelcome!\r\n--------\r\n\n");
+   
     
-    startmain:
+    while(true){
     pc.printf("Please input your error.\r\n");
-    float er = getError();
-    pc.printf("Your error is: %f\r\n",er);
+    //setpoint_1 = getSetpoint_1();
+    //float err_1 = getError_1();
+    float err_1 = getSetpoint_1();
+    pc.printf("Your error is: %f\r\n",err_1);
     pc.printf("-------\r\n-------\r\n");
     wait(0.5);
-    
-
+    outcome_1 = PIDControl_1(err_1);
+    pc.printf("The outcome is %f\r\n",outcome_1);
+    direction_1 = dir_1;
+    checkMovement_1();
+    motor_1();
     
     
-    float outcome = PIDControl(er);
-    pc.printf("The outcome is %f\n",outcome);
+   //only works if error is constantly put in:
+   // if (but_1==0){
+   // goto startmain; //allows to input multiple numbers after each other
+   // }
     
     
-    goto startmain; //allows to input multiple numbers after each other
-
+    }
 }