Diff: main.cpp

Revision:

1:a76fd17e18b3

Parent:

0:335646ab45c0

Child:

2:7ea5ae2287a7

--- a/main.cpp	Mon Oct 28 15:50:03 2019 +0000
+++ b/main.cpp	Mon Oct 28 16:11:15 2019 +0000
@@ -105,72 +105,7 @@
 float gearratio = 131.25; // Gear ratio of gearbox
 
 
-float PID_controller1(float motor1error){
-    static float error_integral1=0;
-    static float e_prev1=motor1error;
-    
-    //Proportional part:
-    u_p1=Kp*motor1error;
-    
-    //Integral part
-    error_integral1=error_integral1+ei1*Ts;
-    u_i1=Ki*error_integral1;
-    
-    //Derivative part
-    float error_derivative1=(motor1error-e_prev1)/Ts;
-    float u_d1=Kd*error_derivative1;
-    e_prev1=motor1error;
-    
-    // Sum and limit
-    up1= u_p1+u_i1+u_d1;
-    if (up1>1){
-        controlsignal1=1;}
-    else if (up1<-1){
-        controlsignal1=-1;}
-    else {
-        controlsignal1=up1;}
-    
-    // To prevent windup
-    ux1= up1-controlsignal1;
-    ek1= Ka*ux1;
-    ei1= motor1error-ek1;
-     //Return
-     return controlsignal1; 
-}
-
-float PID_controller2(float motor2error){
-    static float error_integral2=0;
-    static float e_prev2=motor2error;
-    
-    //Proportional part:
-    u_p2=Kp*motor2error;
-    
-    //Integral part
-    error_integral2=error_integral2+ei2*Ts;
-    u_i2=Ki*error_integral2;
-    
-    //Derivative part
-    float error_derivative2=(motor2error-e_prev2)/Ts;
-    float u_d2=Kd*error_derivative2;
-    e_prev2=motor2error;
-    
-    // Sum and limit
-    up2= u_p2+u_i2+u_d2;
-    if (up2>1){
-        controlsignal2=1;}
-    else if (up2<-1){
-        controlsignal2=-1;}
-    else {
-        controlsignal2=up2;}
-    
-    // To prevent windup
-    ux2= up2-controlsignal2;
-    ek2= Ka*ux2;
-    ei2= motor2error-ek2;
-     //Return
-     return controlsignal2; 
-}
-
+// Vanaf hier is het misschien belangrijk
 
 void readEncoder()
 {
@@ -183,74 +118,36 @@
     countsPrev2 = counts2;    
 }
 
-void togglehoek(){
-  // static float t=0;
-  // Vy=6.0f*sin(50.0f*t);
-  // t+=0.01f;
-   }
-   
-void RKI(){
-  
-    //Vy=potmeter1.read()*10.0*motortoggle;
-    //Vy=potmeter2.read()*10*motortoggle;
-    static float t=0;
-    Vx=6.0f*sin(1.0f*t);
-    t+=0.01f;
-    //Vx=-1.0*;
-    Vy=0.0f;
-    q1dot=(l2*cos(q1+q2)*Vx+l2*sin(q1+q2)*Vy)/((-l1*sin(q1)-l2*sin(q1+q2))*l2*cos(q1+q2)+l2*sin(q1+q2)*(l1*cos(q1)+l2*cos(q1+q2)));
-    q2dot=((-l1*cos(q1)-l2*cos(q1+q2))*Vx+(-l1*sin(q1)-l2*sin(q1+q2))*Vy)/((-l1*sin(q1)-l2*sin(q1+q2))*l2*cos(q1+q2)+l2*sin(q1+q2)*(l1*cos(q1)+l2*cos(q1+q2)));
-    q1=q1+q1dot*Ts;
-    q2=q2+q2dot*Ts;  
-    
-    xe=l1*cos(q1)+l2*cos(q1+q2);
-    ye=l1*sin(q1)+l2*sin(q1+q2);
-    
-    
-    
-    if (q1<0.0f){
-        q1=0.0;}
-    else if (q1>90.0f*deg2rad){
-        q1=90.0f*deg2rad;}    
-    else{
-        q1=q1;}
-    
-    if (q2>-45.0*deg2rad){
-        q2=-45.0*deg2rad;}
-    else if (q2<-135.0*deg2rad){
-        q2=-135.0*deg2rad;}
-    else{
-        q2=q2;}
-    
-    motor1ref=5.5f*q1+5.5f*q2;
-    motor2ref=2.75f*q1;
-    }    
-    
-void motorControl()
+void motorCalibration1()
 {
     button1.fall(&togglehoek);
-    RKI();
-    motor1angle = (counts1 * factorin / gearratio)-(135.0*5.5*deg2rad); // Angle of motor shaft in rad
+    motor1angle = (counts1 * factorin / gearratio); // Angle of motor shaft in rad
     omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
-    motor1error=motor1ref-motor1angle;    
-    controlsignal1=PID_controller1(motor1error);      
-    if (controlsignal1<0){
-        motordir1= 0;}
-    else {
-         motordir1= 1;}
+    float potmeter=potmeter1.read();
+    controlsignal1=potmeter;
     motor1Power.write(abs(controlsignal1*motortoggle));
-    motor1Direction= motordir1;
+    motor1Direction=1;
+    //Dit moet je doen zolang omega motor 1 > 0.iets
+    //State switch
+    //Dan motor 2 tot omega < 0.iets
+    }
+
+void motorCalibration2(){
+ 
+    float potmeter=potmeter1.read();
+    button1.fall(&togglehoek);
+    motor1angle = (counts1 * factorin / gearratio); // Angle of motor shaft in rad
+    omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
+    controlsignal1=potmeter;
+    motor1Power.write(abs(controlsignal1*motortoggle));
+    motor1Direction=1;    
     
     motor2angle = (counts2 * factorin / gearratio); // Angle of motor shaft in rad
-    omega2 = deltaCounts2 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
-    motor2error=motor2ref-motor2angle;    
-    controlsignal2=PID_controller2(motor2error);      
-    if (controlsignal2<0){
-        motordir2= 0;}
-    else {
-         motordir2= 1;}
+    omega2 = deltaCounts2 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s 
+    controlsignal2=potmeter;
     motor2Power.write(abs(controlsignal2*motortoggle));
-    motor2Direction= motordir2;    
+    motor2Direction=1;  
+    //Dit moet je doen zolang omega van motor 2 > 0.iets  
 }
 
 void Plotje(){
@@ -273,26 +170,17 @@
 {
     pc.baud(115200);
     pc.printf("\r\nStarting...\r\n\r\n");
-    
     motor1Power.period(PWM_period);
     motor2Power.period(PWM_period);
-    motorTicker.attach(motorControl, 0.01);
+    motorTicker.attach(motorCalibration1, 0.01);         //Dit moet je doen zolang omega < 0.iets
+    motorTicker.attack(motorCalibration2, 0.01);
     scopeTicker.attach(Plotje, 0.01);
     encoderTicker.attach(readEncoder, Ts);
     
     button2.fall(&toggleMotor);
     
     while (true) {
-//        pc.printf("Angle1:  %f   Omega1:       %f\r\n", motor1angle, omega1);
-//        pc.printf("refangle1: %f   Error1:  %f     \r\n",motor1ref, motor1error);
-//        pc.printf("Angle2:  %f   Omega2:       %f\r\n", motor2angle, omega2);
-//        pc.printf("refangle2: %f   Error2:  %f     \r\n",motor2ref, motor2error);
-//        pc.printf("controlsignal2: %d \r\n", controlsignal2);
-//        pc.printf("Vx: %f Vy: %f \r\n",Vx,Vy);
-//        pc.printf("q1dot: %f q2dot: %f \r\n",q1dot,q2dot);
-            pc.printf("q1: %f q1dot: %f motor1ref: %f motor1angle: %f\r\n", q1*rad2deg, q1dot*rad2deg, motor1ref*rad2deg, motor1angle*rad2deg);
-            pc.printf("q2: %f q2dot: %f motor2ref: %f motor2angle: %f\r\n", q2*rad2deg, q2dot*rad2deg, motor2ref*rad2deg, motor2angle*rad2deg);
-            pc.printf("X: %f Y: %f\r\n",xe,ye);
+            pc.printf("Omega1: %f Omega 2: %f  Potmeter: %f \r\n", omega1, omega2, potmeter);
         wait(0.5);
     }
 }