File content as of revision 10:56136a0da8c1:

#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)

#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

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 contrlling 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;
    }
    
// 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
// Gertjan 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
        }
        
}