Diff: main.cpp

Revision:

9:298469a70280

Parent:

8:3cfc8be293d3

Child:

10:990287a722d2

--- a/main.cpp	Tue Oct 29 16:42:46 2019 +0000
+++ b/main.cpp	Tue Oct 29 18:55:53 2019 +0000
@@ -85,12 +85,11 @@
 HIDScope scope(6);                              //Going to send x channels of data. To access data go to 'http:/localhost:18082/' after starting HIDScope application.
 
 //State maschine
-enum Motor_States{motor_wait , motor_encoderset};
+enum Motor_States{motor_wait , motor_encoderset , motor_wait2 , motor_bewegen};
 Motor_States motor_curr_state;
 bool motor_state_changed = true;
-bool motor_cal1_done = false;
-bool motor_cal2_done = false;
-
+bool motor_calibration_done = false;
+bool motor_RKI=false;
 bool button1_pressed = false;
 bool button2_pressed = false;
 
@@ -139,6 +138,131 @@
     countsPrev2 = counts2;    
 }
 
+void PID_controller(){
+
+    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;
+
+// Motor 2
+    
+    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.0f){
+        controlsignal2=1.0f;}
+    else if (up2<-1){
+        controlsignal2=-1.0f;}
+    else {
+        controlsignal2=up2;}
+    
+    // To prevent windup
+    ux2= up2-controlsignal2;
+    ek2= Ka*ux2;
+    ei2= motor2error-ek2;
+}
+
+void RKI(){
+  
+  if (motor_RKI==true){
+    //Vy=potmeter1.read()*10.0*motortoggle;
+    //Vy=potmeter2.read()*10*motortoggle;
+    static float t=0;
+    Vx=6.0f*sin(1.0f*t);
+    t+=Ts;
+    //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(){
+    motor1angle = (counts1 * factorin / gearratio)-(135.0*5.5*deg2rad); // Angle of motor shaft in rad
+    omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
+    motor1error=motor1ref-motor1angle;         
+    if (controlsignal1<0){
+        motordir1= 0;}
+    else {
+         motordir1= 1;}
+    motor1Power.write(abs(controlsignal1));
+    motor1Direction= motordir1;
+    
+    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;         
+    if (controlsignal2<0){
+        motordir2= 0;}
+    else {
+         motordir2= 1;}
+    motor2Power.write(abs(controlsignal2));
+    motor2Direction= motordir2;    
+}
+
 void do_motor_wait(){
     // Entry function
     if ( motor_state_changed == true ) {
@@ -171,12 +295,52 @@
       
     // State transition guard
 if ( button2_pressed ) {   
-        button2_pressed = false;        
-        motor_curr_state = motor_wait;
+        button2_pressed = false;
+        motor_calibration_done = true;        
+        motor_curr_state = motor_wait2;
         motor_state_changed = true;
     }        
 }
 
+void do_motor_wait2(){
+    // Entry function
+    if ( motor_state_changed == true ) {
+        motor_state_changed = false;
+        // More functions
+    }
+
+    // Do nothing until end condition is met 
+    
+    // State transition guard  
+if ( button2_pressed ) {   
+        button2_pressed = false;      
+        motor_curr_state = motor_encoderset;       
+        motor_state_changed = true;
+        // More functions
+    }    
+   
+}    
+
+void do_motor_bewegen(){
+    // Entry function
+    if ( motor_state_changed == true ) {
+        motor_state_changed = false;
+        // More functions
+    }    
+    
+    motor_RKI=true;
+
+if ( button2_pressed ) {   
+        button2_pressed = false;   
+        motor_RKI = false;  
+        motor_curr_state = motor_wait2;          
+        motor_state_changed = true;
+        // More functions
+    }    
+   
+    
+}
+
 void motor_state_machine()
 {
     switch(motor_curr_state) {
@@ -186,6 +350,12 @@
         case motor_encoderset:
             do_motor_Encoder_Set();
             break;
+        case motor_wait2:
+            do_motor_wait2();
+            break;
+        case motor_bewegen:
+            do_motor_bewegen();
+            break;
     }
 }
 
@@ -196,6 +366,10 @@
     //emg_state_machine();
     motor_state_machine();
     readEncoder();
+    PID_controller();
+    motorControl();
+    RKI();
+    
     // controller();
     // outputToMotors();
 }