Diff: main.cpp

Revision:

3:6e28b992b99e

Parent:

2:7ea5ae2287a7

Child:

4:1e8da6b5f147

--- a/main.cpp	Mon Oct 28 16:38:46 2019 +0000
+++ b/main.cpp	Tue Oct 29 14:04:26 2019 +0000
@@ -84,6 +84,16 @@
 //Hidscope
 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_cal1 , motor_cal2 , motor_encoderset};
+Motor_States motor_curr_state;
+bool motor_state_changed = true;
+bool motor_cal1_done = false;
+bool motor_cal2_done = false;
+
+bool button1_pressed = false;
+bool button2_pressed = false;
+
 // PC connection
 MODSERIAL pc(USBTX, USBRX);
 
@@ -93,6 +103,7 @@
 Ticker directionTicker;
 Ticker encoderTicker;
 Ticker scopeTicker;
+Ticker tickGlobal;      //Global ticker
 
 const float PWM_period = 1e-6;
 
@@ -106,9 +117,13 @@
 float factorin = 6.23185/64; // Convert encoder counts to angle in rad
 float gearratio = 131.25; // Gear ratio of gearbox
 
+void button1Press()
+{
+    button1_pressed = true;
+}
 
-// Vanaf hier is het misschien belangrijk
 
+// Ticker Functions
 void readEncoder()
 {
     counts1 = encoder1.getPulses();        
@@ -120,21 +135,37 @@
     countsPrev2 = counts2;    
 }
 
-void motorCalibration1()
-{
+void do_motorCalibration1() {
+    // Entry function
+    if ( motor_state_changed == true ) {
+        motor_state_changed = false;
+        // More functions
+    }
+
+    // Do stuff until end condition is met
     motor1angle = (counts1 * factorin / gearratio); // Angle of motor shaft in rad
     omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
     float potmeter=potmeter1.read();
     controlsignal1=0.0980f;
     motor1Power.write(abs(controlsignal1*motortoggle));
     motor1Direction=0;
-    //Dit moet je doen zolang omega motor 1 > 0.iets
-    //State switch
-    //Dan motor 2 tot omega < 0.iets
+
+    // State transition guard
+    if ( omega1 <= 0.5f ) {
+        motor_curr_state = motor_cal2;
+        motor_state_changed = true;
+        // More functions
+    }
+}
+
+void do_motorCalibration2(){
+    // Entry function
+    if ( motor_state_changed == true ) {
+        motor_state_changed = false;
+        // More functions
     }
 
-void motorCalibration2(){
- 
+    // Do stuff until end condition is met    
     potmeter=potmeter1.read();
     motor1angle = (counts1 * factorin / gearratio); // Angle of motor shaft in rad
     omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
@@ -148,13 +179,85 @@
     motor2Power.write(abs(controlsignal2*motortoggle));
     motor2Direction=1;  
     //Dit moet je doen zolang omega van motor 2 > 0.iets  
+    
+    // State transition guard
+    if ( omega2 <= 0.5f ) {
+        motor_curr_state = motor_encoderset;
+        motor_state_changed = true;
+        // More functions
+    }    
 }
 
+void do_motor_Encoder_Set(){
+    // Entry function
+    if ( motor_state_changed == true ) {
+        motor_state_changed = false;
+        // More functions
+    }
+
+    // Do stuff until end condition is met 
+    
+    
+    // State transition guard
+    if ( omega2 <= 0.5f ) {         
+        motor_curr_state = motor_encoderset;
+        motor_state_changed = true;
+        // More functions
+    }        
+}
+
+void do_motor_wait(){
+    // Entry function
+    if ( motor_state_changed == true ) {
+        motor_state_changed = false;
+        // More functions
+    }
+
+    // Do nothing until end condition is met 
+    
+    // State transition guard
+    if ( button1_pressed ) {   
+        button1_pressed = false;      
+        motor_curr_state = motor_cal1;           //Beginnen met calibratie
+        motor_state_changed = true;
+        // More functions
+    }        
+}    
+
 void toggleMotor()
 {
     motortoggle = !motortoggle;
 }
 
+void motor_state_machine()
+{
+    switch(motor_curr_state) {
+        case motor_wait:
+            do_motor_wait();
+            break;
+        case motor_cal1:
+            do_motorCalibration1();
+            break;
+        case motor_cal2:
+            do_motorCalibration2();
+            break;
+        case motor_encoderset:
+            do_motor_Encoder_Set();
+            break;
+    }
+}
+
+// Global loop of program
+void tickGlobalFunc()
+{
+    //sampleSignal();
+    //emg_state_machine();
+    motor_state_machine();
+    // controller();
+    // outputToMotors();
+}
+
+
 int main()
 {
     pc.baud(115200);
@@ -162,9 +265,8 @@
     motor1Power.period(PWM_period);
     motor2Power.period(PWM_period);
     
-    motorTicker.attach(motorCalibration1, 0.01);         //Dit moet je doen zolang omega < 0.iets
-    //motorTicker.attack(motorCalibration2, 0.01);
-    encoderTicker.attach(readEncoder, Ts);
+    motor_curr_state = motor_wait;                          // Start off in EMG Wait state
+    tickGlobal.attach( &tickGlobalFunc, Ts );
     
     button2.fall(&toggleMotor);