Diff: THE.cpp

Revision:

11:79311abb2bc2

Parent:

10:56136a0da8c1

Child:

12:b2b082e73ef1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/THE.cpp	Wed Oct 31 13:29:06 2018 +0000
@@ -0,0 +1,414 @@
+#include "mbed.h" // Use revision 119!!
+#include "HIDScope.h" // For displaying data, select MBED - HID device, restart for every new code 
+#include "QEI.h" // For reading the encoder of the motors 
+#include <ctime> // for the timer during the process (if needed)
+#include "Servo.h" // For controlling the servo
+
+#define SERIAL_BAUD 115200
+
+// In- en outputs 
+// -----------------------------------------------------------------------------
+
+// EMG related
+AnalogIn emg1(); // EMG signal 1
+AnalogIn emg2(); // EMG signal 2
+// if needed
+AnalogIn emg3(); // EMG signal 3
+AnalogIn emg4(); // EMG signal 4
+
+
+// Motor related
+DigitalOut dirpin_1(D4); // direction of motor 1
+PwmOut pwmpin_1(D5); // PWM pin of motor 1
+DigitalOut dirpin_2(D7); // direction of motor 2
+PwmOut pwmpin_2(D6); // PWM pin of motor 2
+
+// Extra stuff
+// Like LED lights, buttons etc 
+
+DigitalIn button_motorcal(SW2); // button for motor calibration, on mbed
+DigitalIn button_emergency(D7); // button for emergency mode, on bioshield
+DigitalIn button_wait(SW3); // button for wait mode, on mbed 
+DigitalIn button_demo(D6); // button for demo mode, on bioshield  
+
+DigitalIn led_red(LED_RED); // red led 
+DigitalIn led_green(LED_GREEN); // green led
+DigitalIn led_blue(LED_BLUE); // blue led
+
+AnalogIn pot_1(A1); // potmeter for simulating EMG input
+
+
+// Other stuff
+// -----------------------------------------------------------------------------
+// Define stuff like tickers etc
+Servo myservo(D7); // Define the servo to control (in penholder)
+Ticker ticker; // Name a ticker, use each ticker only for 1 function! 
+HIDScope scope(2); // Number of channels in HIDScope
+QEI Encoder(D13,D12,NC,64,QEI::X4_ENCODING); // Define the type of encoding: X4 encoding(default is X2)
+Serial pc(USBTX,USBRX);
+Timer t; // For timing the time in each state (https://os.mbed.com/handbook/Timer)
+
+// Variables 
+// ----------------------------------------------------------------------------- 
+// Define here all variables needed throughout the whole code 
+int counts;
+float potmeter_value;
+double time_overall;
+float time_in_state;
+double motor_velocity = 0;
+double EMG = 0;
+double errors = 0;
+
+// states
+enum states {WAIT, MOTOR_CAL, EMG_CAL, START, OPERATING, FAILURE, DEMO}; // states the robot can be in
+states CurrentState = WAIT; // the CurrentState to start with is the WAIT state
+bool StateChanged = true; // the state must be changed to go into the next state
+
+// Functions
+// -----------------------------------------------------------------------------
+ 
+// Encoder 
+// Getting encoder information from motors
+int encoder()
+{
+    int counts = Encoder.getPulses();
+    return counts;
+    } 
+    
+// Potmeter for controlling motor 
+float potmeter()
+{
+    float out_1 = pot_1 * 2.0f;
+    float out_2 = out_1 - 1.0f;
+                
+    dirpin_1.write(out_2 < 0);
+    dirpin_2.write(out_2 < 0);
+    
+    pwmpin_1 = fabs (out_2); // Has to be positive value
+    pwmpin_2 = fabs (out_2);
+        
+    return out_2;
+    }
+    
+// Servo control
+// To lift the pen up, with a push of button 2
+void servocontrol()
+{
+    if (but_1)
+    {
+        myservo = 0.1;
+        }
+    else 
+    {
+        myservo = 0.0;
+        } 
+}
+    
+// Send information to HIDScope
+void hidscope() // Attach this to a ticker! 
+{
+    scope.set(0, counts); // send EMG 1 to channel 1 (=0)
+    scope.set(1, potmeter_value); // sent EMG 2 to channel 2 (=1)
+    
+    // Ensure that enough channels are available (HIDScope scope(2))
+    scope.send(); // Send all channels to the PC at once
+    }
+    
+// EMG filter
+// To process the EMG signal before information can be caught from it 
+// Kees mee bezig 
+
+// WAIT
+// To do nothing
+void wait_mode()
+{
+    // go back to the initial values
+    // Copy here the variables list with initial values
+    motor_velocity = 0;
+    }
+
+// MOTOR_CAL
+// To calibrate the motor angle to some mechanical boundaries
+// Kenneth mee bezig 
+void motor_calibration()
+{
+    // Kenneths code here
+    }
+
+// EMG_CAL
+// To calibrate the EMG signal to some boundary values
+void emg_calibration()
+{
+    // code
+    }
+    
+// PID controller
+// To control the input signal before it goes into the motor control
+// Simon mee bezig
+void pid_controller()
+{
+    // Simons code here
+    }
+
+// START
+// To move the robot to the starting position: middle
+void start_mode() 
+{
+    // move to middle
+    }
+
+// OPERATING
+// To control the robot with EMG signals
+// Kenneth bezig met motor aansturen
+
+// DEMO
+// To control the robot with a written code and write 'HELLO'
+// Voor het laatst bewaren
+void demo_mode()
+{
+    // code here
+    }
+
+// FAILURE
+// To shut down the robot after an error etc 
+void failure()
+{
+    // code here
+    }
+
+// Main function
+// -----------------------------------------------------------------------------
+int main()
+{
+    pc.baud(115200); // For TeraTerm, the baudrate, set also in TeraTerm itself! 
+    pc.printf("Start code\r\n"); // To check if the program starts 
+    pwmpin_1.period_us(60); // Setting period for PWM
+    ticker.attach(&hidscope,0.002f); // Send information to HIDScope
+    
+    while(true){
+        // timer
+        clock_t start; // start the timer
+        start = clock(); 
+        time_overall = (clock() - start) / (double) CLOCKS_PER_SEC;
+        
+        counts = encoder();
+        potmeter_value = potmeter();
+         
+        
+        //pc.printf("potmeter value = %f ", potmeter_value);
+        //pc.printf("counts = %i\r\n", counts); 
+        
+        // With the help of a switch loop and states we can switch between states and the robot knows what to do
+        switch(CurrentState)
+        {
+            case WAIT:
+            
+            if(StateChanged) // so if StateChanged is true
+            {
+                pc.printf("State is WAIT\r\n");
+                
+                // Execute WAIT mode
+                wait_mode();
+                
+                StateChanged = false; // the state is still WAIT
+                }
+                
+            if(button_motorcal == true) // condition for WAIT --> MOTOR_CAl; button_motorcal press
+            {
+                CurrentState = MOTOR_CAL;
+                pc.printf("State is MOTOR_CAL\r\n");
+                StateChanged = true;
+                }
+                
+            if (button_emergency == true) // condition for WAIT --> FAILURE; button_emergency press 
+            {
+                CurrentState = FAILURE;
+                pc.printf("State is FAILURE\r\n");
+                StateChanged = true;
+                }
+            
+            break;
+            
+            case MOTOR_CAL:
+            
+            if(StateChanged) // so if StateChanged is true
+            {
+                t.reset();
+                t.start();
+                
+                // Execute MOTOR_CAL mode
+                motor_calibration();
+                
+                t.stop();
+                time_in_state = t.read();
+                pc.printf("Time here = %f\r\n", time_in_state);
+                
+                StateChanged = false; // the state is still MOTOR_CAL
+                }
+            
+            if((time_in_state > 3.0) && (motor_velocity < 0.01)) // condition for MOTOR_CAL --> EMG_CAL; 3s en motors stopped moving
+            {
+                CurrentState = EMG_CAL;
+                pc.printf("State is EMG_CAL\r\n");
+                StateChanged = true;
+                }
+                
+            if (button_emergency == true) // condition for MOTOR_CAL --> FAILURE; button_emergency press 
+            {
+                CurrentState = FAILURE;
+                pc.printf("State is FAILURE\r\n");
+                StateChanged = true;
+                }
+                
+            break;
+            
+            case EMG_CAL:
+            
+            if(StateChanged) // so if StateChanged is true
+            {
+                t.reset();
+                t.start();
+                
+                // Execute EMG_CAL mode
+                emg_calibration();
+                
+                t.stop();
+                time_in_state = t.read();
+                pc.printf("Time here = %f\r\n", time_in_state);
+                
+                StateChanged = false; // state is still EMG_CAL
+                }
+            
+            if((time_in_state > 5) && (EMG < 0.01)) // condition for EMG_CAL --> START; 5s and EMG is low
+            {
+                CurrentState = START;
+                pc.printf("State is START\r\n");
+                StateChanged = true;
+                }
+                
+            if (button_emergency == true) // condition for EMG_CAL --> FAILURE; button_emergency press 
+            {
+                CurrentState = FAILURE;
+                pc.printf("State is FAILURE\r\n");
+                StateChanged = true;
+                }
+                
+            break;
+            
+            case START:
+            
+            if(StateChanged) // so if StateChanged is true
+            {
+                t.reset();
+                t.start();
+                
+                // Execute START mode
+                start_mode();
+                
+                t.stop();
+                time_in_state = t.read();
+                pc.printf("Time here = %f\r\n", time_in_state);
+                
+                StateChanged = false; // state is still START
+                }
+            
+            if((time_in_state > 5) && (errors < 0.01)) // condition for START --> OPERATING; 5s and error is small
+            {
+                CurrentState = OPERATING;
+                pc.printf("State is OPERATING\r\n");
+                StateChanged = true;
+                }
+                
+            if (button_emergency == true) // condition for START --> FAILURE; button_emergency press 
+            {
+                CurrentState = FAILURE;
+                pc.printf("State is FAILURE\r\n");
+                StateChanged = true;
+                }
+                
+            break;
+            
+            case OPERATING:
+            
+            if(StateChanged) // so if StateChanged is true
+            {
+                // Execute OPERATING mode
+                
+                StateChanged = false; // state is still OPERATING
+                }
+            
+            if(button_emergency == true) // condition for OPERATING --> FAILURE; button_emergency press
+            {
+                CurrentState = FAILURE;
+                pc.printf("State is FAILURE\r\n");
+                StateChanged = true;
+                }
+                
+            if(button_demo == true) // condition for OPERATING --> DEMO; button_demo press
+            {
+                CurrentState = DEMO;
+                pc.printf("State is DEMO\r\n");
+                StateChanged = true;
+                }
+                
+            if(button_wait == true) // condition OPERATING --> WAIT; button_wait press  
+            {
+                CurrentState = WAIT;
+                pc.printf("State is WAIT\r\n");
+                StateChanged = true;
+                }
+                
+            break;
+            
+            case FAILURE:
+            
+            if(StateChanged) // so if StateChanged is true
+            {
+                // Execute FAILURE mode
+                failure_mode();
+                
+                StateChanged = false; // state is still FAILURE
+                }
+            
+            if(button_wait == true) // condition for FAILURE --> WAIT; button_wait press (IF THAT IS EVEN POSSIBLE IN THIS STATE?) 
+            {
+                CurrentState = WAIT;
+                pc.printf("State is WAIT\r\n");
+                StateChanged = true;
+                }
+                
+            break;
+            
+            case DEMO:
+            
+            if(StateChanged) // so if StateChanged is true
+            {
+                // Execute DEMO mode
+                demo_mode();
+                
+                StateChanged = false; // state is still DEMO
+                }
+            
+            if(button_wait == true) // condition for DEMO --> WAIT; button_wait press 
+            {
+                CurrentState = WAIT;
+                pc.printf("State is WAIT\r\n");
+                StateChanged = true;
+                }
+                
+            if (button_emergency == true) // condition for DEMO --> FAILURE; button_emergency press 
+            {
+                CurrentState = FAILURE;
+                pc.printf("State is FAILURE\r\n");
+                StateChanged = true;
+                }
+                
+            break;
+            
+            // no default  
+            }
+            
+        // while loop does not have to loop every time
+        }
+        
+}
\ No newline at end of file