Diff: main.cpp

Revision:

8:c7d3b67346db

Parent:

7:1d57463393c6

Child:

9:4de589636f50

--- a/main.cpp	Fri Oct 11 12:31:34 2019 +0000
+++ b/main.cpp	Mon Oct 14 09:46:14 2019 +0000
@@ -10,72 +10,44 @@
 //definieer objecten
 Serial pc(USBTX, USBRX);
 
-//Servo myservo(D8);
-
-Ticker myControllerTicker1;
-Ticker myControllerTicker2;
-
-AnalogIn potMeter1(A1);
-AnalogIn potMeter2(A0);
-
 PwmOut motor1(D6);  
 PwmOut motor2(D5);  
 
-DigitalOut motor1_dir(D7);
-DigitalOut motor2_dir(D4);
-
-
-InterruptIn button1(D1);
-InterruptIn button2(D2);
+InterruptIn Power_button_pressed(D1);
+InterruptIn Emergency_button_pressed(D2);
 
-//richting wisselen van motor 1
-void direction1(void)
-    {
-        motor1_dir=!motor1_dir;
-    }
-
-//richting wisselen van motor 2
-void direction2(void)
-    {
-        motor2_dir=!motor2_dir;
-    }
-
-
-//snelheid motor 1 aansturen
-void motor1Controller( )
+Ticker loop_ticker;
+    
+// Motoren uitzetten
+void motors_on_off()
     {
-    // Determine reference (desired) fan speed
-        double y1_des = potMeter1.read();
-    // Controller (calculate motor torque/pwm)
-        if( y1_des > 1 ) y1_des = 1; // y1_des must be <= 1
-        if( y1_des < 0 ) y1_des = 0; // y1_des must be >= 0
-        double power1 = pow(y1_des, 2.0); // Inverse relation between input and output
-    // Send to motor
-        motor1.write( power1 );
-        pc.printf("power1: %.2f\t",power1);
-    }
-
-//snelheid motor 2 aansturen
-void motor2Controller( )
+          motor1=!motor1;
+          motor2=!motor2;
+          pc.printf("Motoren staan aan/uit");
+    }    
+    
+void emergency()
     {
-    // Determine reference (desired) fan speed
-        double y2_des = potMeter2.read();
-    // Controller (calculate motor torque/pwm)
-        if( y2_des > 1 ) y2_des = 1; // y2_des must be <= 1
-        if( y2_des < 0 ) y2_des = 0; // y2_des must be >= 0
-        double power2 = pow(y2_des, 2.0); // Inverse relation between input and output
-    // Send to motor
-        motor2.write( power2 );
-        pc.printf("power2: %.2f\n\r",power2);
-    }
+        motor1.write(0);
+        motor2.write(0);
+        pc.printf("Noodgeval");
+    }    
+    
+void motors_off()
+    {
+        motor1.write(0);
+        motor2.write(0);
+        pc.printf("Motoren staan uit");
+    }  
+    
+    
 
 // Finite state machine programming (calibration servo motor?)
-enum states {Motors_off, Calib_motor, Calib_EMG, Homing, Operation_mode, Failure_mode};
+enum states {Motors_off, Failure_mode};
 
 states currentState = Motors_off; 
 bool stateChanged = true; // Make sure the initialization of first state is executed
 
-
 void ProcessStateMachine(void) 
 { 
     switch (currentState) 
@@ -84,110 +56,34 @@
         
             if (stateChanged)
             {
-                // functie toevoegen waarbij motoren uitgaan
+                motors_off(); // functie waarbij motoren uitgaan
                 stateChanged = false;
                 pc.printf("Motors off\n");
             }            
-            if // (Power_button_pressed()) nog een button toevoegen voordat dit kan
+            if  (Power_button_pressed)
             {      
-                currentState = Calib_motor;
+                Power_button_pressed.mode(PullUp); 
+                Power_button_pressed.rise(motors_on_off); // functie waarbij de motoren aan-/ uitgaan 
                 stateChanged = true;
                 pc.printf("Moving to Calib_motor state\n");
             }
-            if // (Emergency_button_pressed()) nog een button toevoegen voordat dit kan
-            {
-                currentState = Failure_mode;
-                stateChanged = true;
-                pc.printf("Moving to failure_mode\n");
-            }
-                
-            break;
-            
-        case Calib_motor:
-            
-            if (stateChanged)
-            {       
-                // Hier wordt een kalibratie uitgevoerd, waarbij de motorhoeken worden bepaald
-                currentState = Calib_EMG;
-                stateChanged = true;
-                pc.printf("Moving to Calib_EMG state\n");
-            }   
-            if // (Emergency_button_pressed()) nog een button toevoegen voordat dit kan
-            {
+            if  (Emergency_button_pressed) 
+            { 
+                Emergency_button_pressed.mode(PullUp); 
+                Emergency_button_pressed.rise(emergency); // functie die de motoren uitzet
                 currentState = Failure_mode;
                 stateChanged = true;
                 pc.printf("Moving to failure_mode\n");
             }
                 
             break;
-            
-        case Calib_EMG:
-            
-            if (stateChanged)
-            {
-                // Hierbij wordt een een kalibratie uitgevoerd, waarbij de maximale EMG-amplitude waarde wordt bepaald
-                currentState = Homing;
-                stateChanged = true;
-                pc.printf("Moving to Homing state\n");
-            }
-            if // (Emergency_button_pressed()) nog een button toevoegen voordat dit kan
-            {
-                currentState = Failure_mode;
-                stateChanged = true;
-                pc.printf("Moving to failure_mode\n");
-            }
-            
-            break;
-            
-        case Homing:
-            
-            if (stateChanged)
-            {   
-                // Ervoor zorgen dat de motoren zo bewegen dat de robotarm 
-                // (inclusief de end-effector) in de juiste home positie wordt gezet
-                currentState = Operation_mode;
-                stateChanged = true;
-                pc.printf("Moving to operation mode\n");
-            }
-            if // (Emergency_button_pressed()) nog een button toevoegen voordat dit kan
-            {
-                currentState = Failure_mode;
-                stateChanged = true;
-                pc.printf("Moving to failure_mode\n");
-            }   
-            break;
-                  
-        case Operation_mode:
-        
-            if (stateChanged)
-            { 
-                // Hier moet een functie worden aangeroepen die ervoor zorgt dat 
-                // aan de hand van EMG-signalen de motoren kunnen worden aangestuurd, 
-                // zodat de robotarm kan bewegen   
-                
-                if // (Power_button_pressed())
-                {
-                    currentState = Motors_off;
-                    stateChanged = true;
-                }
-                if // (Emergency_button_pressed()) nog een button toevoegen voordat dit kan
-                {
-                    currentState = Failure_mode;
-                    stateChanged = true;
-                    pc.printf("Moving to failure_mode\n");     
-                }
-                else 
-                { 
-                    currentState = Homing; 
-                    stateChanged = true; 
-                }
-                break;
                 
         case Failure_mode:
         
             if (stateChanged)
             { 
                 pc.printf("Ik ga exploderen!!!");
+                loop_ticker.detach(); 
                 // Alles moet uitgaan (evt. een rood LEDje laten branden), moet 
                 // opnieuw worden opgestart. Mogelijk kan dit door de ticker de 
                 // detachen
@@ -197,28 +93,13 @@
         default:
             // Zelfde functie als die eerder is toegepast om motoren uit te schakelen -> safety!
             pc.printf("Unknown or uninplemented state reached!\n");
+            
         }
-
-int main()
-    {
-      //  for(float p=0; p<1.0; p += 0.1)
-        //    {
-        //        myservo == p;
-         //       wait(0.2);
-         //   }
-        myControllerTicker1.attach(motor1Controller, 0.1 ); // Every 1/10 second
-        myControllerTicker2.attach(motor2Controller, 0.1 ); // Every 1/10 second
-
-        button1.mode(PullUp);   
-        button1.rise(direction1);
-        button2.mode(PullUp);   
-        button2.rise(direction2);
-
-
-        while( true ) { /* do nothing */ }
-    return 0;
     }
 
-
-
-
+int main(void)
+    {
+        loop_ticker.attach(&ProcessStateMachine, 2.0f);  
+        while(true) 
+        { /* do nothing */ ;}
+    }
\ No newline at end of file