Diff: main.cpp

Revision:

15:75070aedf4b7

Parent:

14:8aa57a7c5b30

Child:

16:6334ad516980

--- a/main.cpp	Wed Oct 30 12:52:26 2019 +0000
+++ b/main.cpp	Wed Oct 30 13:32:33 2019 +0000
@@ -9,7 +9,7 @@
 InterruptIn button1(D11);
 InterruptIn button2(D10);
 InterruptIn buttonsw2(SW2);
-InterruptIn buttonsw3(SW3);
+InterruptIn buttonfailure(SW3);
 AnalogIn potmeter1(A0);
 AnalogIn potmeter2(A1);
 AnalogIn potmeter3(A2);
@@ -86,7 +86,7 @@
 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 , motor_wait2 , motor_bewegen};
+enum Motor_States{motor_wait , motor_encoderset , motor_wait2 , motor_bewegen , failuremode};
 Motor_States motor_curr_state;
 bool motor_state_changed = true;
 bool motor_calibration_done = false;
@@ -207,8 +207,6 @@
 }
 
 void RKI(){
-  
-  if (motor_RKI==true){
     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;
@@ -219,8 +217,8 @@
     
     
     
-    if (q1<0.0f){
-        q1=0.0;}
+    if (q1<-5.0f){
+        q1=-5.0;}
     else if (q1>65.0f*deg2rad){
         q1=65.0f*deg2rad;}    
     else{
@@ -228,14 +226,14 @@
     
     if (q2>-50.0*deg2rad){
         q2=-50.0*deg2rad;}
-    else if (q2<-135.0*deg2rad){
-        q2=-135.0*deg2rad;}
+    else if (q2<-138.0*deg2rad){
+        q2=-138.0*deg2rad;}
     else{
         q2=q2;}
     
     motor1ref=5.5f*q1+5.5f*q2;
     motor2ref=2.75f*q1;
-    }
+    
 }    
     
 
@@ -272,7 +270,8 @@
         // More functions
     }
 
-    // Do nothing until end condition is met 
+    PID_controller();
+    motorControl();
     
     // State transition guard  
 if ( button2_pressed ) {   
@@ -311,7 +310,10 @@
         // More functions
         }
         
-     motor_RKI=true;
+    PID_controller();
+    motorControl();
+    RKI();   
+    
     // Do nothing until end condition is met 
     
     // State transition guard  
@@ -330,11 +332,15 @@
         motor_state_changed = false;
         // More functions
     }    
+    
     static float t=0;
     Vx=10.0f*sin(1.0f*t);
     Vy=0.0f;
+    t+=Ts;
     
-    t+=Ts;
+    PID_controller();
+    motorControl();
+    RKI();
    
 if ( button2_pressed ) {   
         button2_pressed = false;   
@@ -347,6 +353,21 @@
     
 }
 
+void do_failuremode(){
+    // Entry function
+    if ( motor_state_changed == true ) {
+        motor_state_changed = false;
+        // More functions
+    }    
+        motor_curr_state = failuremode;
+        
+        motor1Power.write(0.0f);
+        motor2Power.write(0.0f);
+        Vx=0.0f;
+        Vy=0.0f;
+    
+    }
+
 void motor_state_machine()
 {
     switch(motor_curr_state) {
@@ -372,9 +393,6 @@
     //emg_state_machine();
     motor_state_machine();
     readEncoder();
-    PID_controller();
-    motorControl();
-    RKI();
     
     // controller();
     // outputToMotors();
@@ -393,6 +411,7 @@
     
     button1.fall(&button1Press);
     button2.fall(&button2Press);
+    buttonfailure.fall(&do_failuremode);
     
     while (true) {
             pc.printf("counts1offset %i counts1af: %i counts1: %i\r\n",counts1offset,counts1af,counts1);