Diff: main.cpp

Revision:

11:bda77916d2ea

Parent:

8:9edf32e021a5

Child:

12:5be2001abe62

--- a/main.cpp	Mon Oct 23 09:22:38 2017 +0000
+++ b/main.cpp	Fri Oct 27 10:00:20 2017 +0000
@@ -8,9 +8,14 @@
 
 MODSERIAL pc(USBTX,USBRX);
 
+
+enum r_states {R_HORIZONTAL, R_VERTICAL, R_BASE};
+r_states state;
+
 // ---- EMG parameters ----
 //HIDScope scope (2); 
 EMG_filter emg1(A0);
+EMG_filter emg2(A1);
 // ------------------------ 
 
 // ---- Motor input and outputs ----
@@ -19,10 +24,13 @@
 DigitalOut dir1(D4);
 DigitalOut dir2(D7);
 DigitalIn press(PTA4);
-DigitalOut led1(D8);
-DigitalOut led2(D11);
+DigitalOut ledred(LED_RED);
+DigitalOut ledgreen(LED_GREEN);
+DigitalOut ledblue(LED_BLUE);
+DigitalOut ledstateswitch(D8);
+DigitalOut ledstatedef(D11);
 AnalogIn pot(A2);
-AnalogIn pot2(A1);
+AnalogIn pot2(A3);
 Encoder motor1(PTD0,PTC4);
 Encoder motor2(D12,D13);
 // ----------------------------------
@@ -43,7 +51,7 @@
 
 double setpoint = 6.28;//I am setting it to move through 180 degrees
 double setpoint2 = 6.28;//I am setting it to move through 180 degrees
-double Kp = 250;// you can set these constants however you like depending on trial & error
+double Kp = 500;// you can set these constants however you like depending on trial & error
 double Ki = 100;
 double Kd = 0;
 
@@ -69,7 +77,9 @@
 // --- Booleans for the maintickerfunction ---
 //bool readoutsetpoint = true;
 bool go_EMG;                    // Boolean to put on/off the EMG readout
-bool go_calibration;            // Boolean to put on/off the calibration of the EMG   
+bool go_calibration;            // Boolean to put on/off the calibration of the EMG 
+bool go_switch;                  // Boolean to go to different state  
+bool go_PID;                    // Boolean to calculate PID and motor aansturing
 // -------------------------------------------
 
 // --- calibrate EMG signal ----
@@ -77,7 +87,7 @@
 void calibrationGO()                   // Function for go or no go of calibration
 {
     go_calibration = false;
-    led2 = 0;
+    
 }
 
 /*
@@ -90,46 +100,96 @@
 {
     if (go_calibration)
     {
-        led2 = 1;
-        //emg1.calibration();                 // Using the calibration function of the EMG_filter class              
+        
+        emg1.calibration();                 // Using the calibration function of the EMG_filter class  
+        emg2.calibration();                             
     }
 }
 
 // ------------------------------
 
-void setpointreadout()
+// --- motor movements ---
+void r_movehorizontal()
+{
+
+
+}
+
+void r_movevertical()
+{
+
+}
+
+void r_movebase()
+{
+
+}
+//--------------------------------
+
+//--- State switch function -----
+void r_processStateSwitch()
 {
-    
-    potvalue = pot.read();
-    setpoint = potvalue*6.28f;
-    
-    potvalue2 = pot2.read();
-    setpoint2 = potvalue2*6.28f;
-   
+    if(go_switch) {                          //if go_switch is true state is switched
+        go_switch = false;
+        switch(state) {
+            case R_HORIZONTAL:
+                state = R_VERTICAL;
+                ledblue = 1;
+                ledred = 1;
+                ledgreen = 0;
+                pc.printf("state is vertical");
+                break;
+            case R_VERTICAL:
+                state = R_BASE;
+                ledgreen = 1;
+                ledblue = 1;
+                ledred = 0;
+                pc.printf("state is base");
+                break;
+            case R_BASE:
+                state = R_HORIZONTAL;
+                ledgreen = 1;
+                ledred = 1;
+                ledblue = 0;
+                pc.printf("state is horizontal");
+                break;
+                }
+                wait(1.0f);                             // waits 1 second to continue with the main function. I think ticker does run, but main is on hold.
+                ledstatedef = 1;
+                ledstateswitch = 0;
+        }   
 }
 
 //Function that reads out the raw EMG and filters the signal 
 void processEMG ()
 {
-        led1 = 1;
-        /*
-        //go_EMG = false;                   //set the variable to false --> misschien nodig?
+        
+
         emg1.filter();                      //filter the incoming emg signal
-        //emg2.filter();
+        emg2.filter();
         //emg3.filter();
         
-        scope.set(0, emg1.normalized);      //views emg1.normalized on port 0 of HIDScope  
+      /*  scope.set(0, emg1.normalized);      //views emg1.normalized on port 0 of HIDScope  
         scope.set(1, emg1.emg0);        
         scope.send();*/
    
 }
  
+void setpointreadout()
+{
+    potvalue = pot.read();
+    setpoint = emg1.normalized;
+    
+    potvalue2 = pot2.read();
+    setpoint2 = emg2.normalized;
+   
+}
    
 
 void PIDcalculation() // inputs: potvalue, motor#, setpoint
 {
     setpointreadout();
-    angle = motor1.getPosition()/4200.00*6.28;   
+    angle = motor1.getPosition()/4200.00;   
     angle2 = motor2.getPosition()/4200.00*6.28;
    
     //pc.printf("pos: %d, speed %f reference position = %.2f\r ",motor1.getPosition(), motor1.getSpeed(), setpoint);
@@ -189,36 +249,70 @@
 }
 
 void maintickerfunction()
-{     
+{    
+    r_processStateSwitch();
+     
     if(go_EMG)
     {
         processEMG();
     }
+    /*if(emg1.normalized >=0.2 && emg2.normalized >= 0.2) // PIDcalculation should not happen. 
+    { 
+        go_PID = false;
+    }
+    else{go_PID = true;}*/
     
-    PIDcalculation();    
+    if(go_PID){
+    PIDcalculation(); 
+    } 
 }
 
 int main()
 {   
+    pc.baud(9600);   
     go_EMG = true;                      // Setting ticker variables    
     go_calibration = true;              // Setting the timeout variable
+    go_PID = true;
+    go_switch = false;
     speed1.period(PwmPeriod);
     speed2.period(PwmPeriod);
     
     calibrationgo.attach(&calibrationGO, 5.0);     // Attach calibration timeout to calibration function
     mainticker.attach(&calibrationEMG, 0.001f);      // Attach ticker to the calibration function
-    wait(5.0f);
-    
+    wait(5.0f);   
     mainticker.attach(&maintickerfunction,0.001f);        
     
-    int count = 0;    
+    int count = 0;  
+      
     while(true) {
+       
+        ledstatedef = 1;
+        if(emg1.normalized >= 0.7 && emg2.normalized >= 0.7) {
+            ledstateswitch = 1;
+            ledstatedef = 0;
+            go_switch = true;
+                      
+        }
         
+
+        switch(state) {
+            case R_HORIZONTAL:
+                r_movehorizontal();
+                break;
+            case R_VERTICAL:
+                r_movevertical();
+                break;
+            case R_BASE:
+                r_movebase();
+                break;
+                       }
+
         count++;
         if(count == 100){
             count = 0;
-            pc.printf("Angle = %f, pidTerm = %f ,PID_scaled = %f, Error = %f, setpoint = %f\r\n", angle, pidTerm ,pidTerm_scaled, error1,setpoint);
-            pc.printf("Angle 2 = %f, pidTerm 2 = %f ,PID_scaled 2 = %f, Error 2 = %f, setpoint 2 = %f\r\n", angle2, pidTerm2 ,pidTerm_scaled2, error2,setpoint2);
+            pc.printf("emg1 = %f, emg2 = %f\r\n", emg1.normalized, emg2.normalized);
+            //pc.printf("Angle = %f, pidTerm = %f ,PID_scaled = %f, Error = %f, setpoint = %f\r\n", angle, pidTerm ,pidTerm_scaled, error1, setpoint);
+            //pc.printf("Angle 2 = %f, pidTerm 2 = %f ,PID_scaled 2 = %f, Error 2 = %f, setpoint 2 = %f\r\n", angle2, pidTerm2 ,pidTerm_scaled2, error2,setpoint2);
         }