File content as of revision 1:afb820c6fc0d:

#include "mbed.h"
#include "QEI.h"
#include "HIDScope.h"
#include "MODSERIAL.h"

PwmOut pwmpin1(D6);
PwmOut pwmpin2(D5);
AnalogIn potmeter1(A5);
AnalogIn potmeter2(A4);
DigitalIn button1(D2);
DigitalIn button2(D3);
DigitalOut directionpin1(D4);
DigitalOut directionpin2(D7);
QEI motor1(D13,D12,NC, 32);
QEI motor2(D11,D10,NC, 32);

DigitalIn kill_switch(SW2); //position has to be changed
DigitalIn next_switch(SW3); //name and position should be replaced

//for testing purposes
DigitalOut ledred(LED_RED);
DigitalOut ledgreen(LED_GREEN);
DigitalOut ledblue(LED_BLUE);

enum states{PositionCalibration, EmgCalibration, Movement, KILL};
states CurrentState;
MODSERIAL pc(USBTX, USBRX);
//HIDScope scope(2);

Ticker MotorsTicker;

volatile float pwm_value1 = 0.0;
volatile float pwm_value2 = 0.0;

void positionCalibration() {
    while(!button1){
        directionpin2 = true;
        pwm_value2 = 1.0f;
    } 
    pwm_value2 = 0.0f;
    while(!button2){
        directionpin2 = true;
        pwm_value2 = 0.7f;
        //wait(0.01f);
    }
    pwm_value2 = 0.0f;
        
    // pwm_value1 = potmeter1;
    // pwm_value2 = potmeter2;
    
    if (!next_switch) {
        CurrentState = EmgCalibration;
        pc.printf("current state = EmgCalibration\n\r");
    }
}

void emgCalibration() {
    ledblue = !ledblue;
    wait(0.5f);
    
    if (!next_switch) {
        CurrentState = Movement;
        pc.printf("current state = Movement\n\r");
    }
}

void movement() {
    
}

void move_motors() {
    pwmpin1 = pwm_value1;
    pwmpin2 = pwm_value2;
}

int main()
{
    pc.baud(115200);
    pc.printf(" ** program reset **\n\r");
    pwmpin1.period_us(60);
    pwmpin2.period_us(60);
    directionpin1 = true;
    directionpin2 = true;
    ledred = true;
    ledgreen = true;
    ledblue = true;
    
    MotorsTicker.attach(&move_motors, 0.02f);
    CurrentState = PositionCalibration;
    pc.printf("current state = PositionCalibration\n\r");
    
    while (true) {
        switch(CurrentState) {
            case PositionCalibration:
                ledred = true;
                ledgreen = false;
                ledblue = true;
                positionCalibration();
                
                if (!kill_switch) {
                    CurrentState = KILL;   
                    pc.printf("current state = KILL\n\r"); 
                }
            break;
                
            case EmgCalibration:
                ledred = true;
                ledgreen = true;
                // ledblue = false;
                emgCalibration();
                
                if (!kill_switch) {
                    CurrentState = KILL;   
                    pc.printf("current state = KILL\n\r");  
                }
            break;
            
            case Movement:
                ledred = true;
                ledgreen = false;
                ledblue = false;
                movement();
                
                if (!kill_switch) {
                    CurrentState = KILL; 
                    pc.printf("current state = KILL\n\r");     
                }
            break;
            
            case KILL:
                bool u = true;
                ledgreen = true;
                ledblue = true;
                ledred = false;
                //turnoffmotors(); //placeholder
                //flashsos() //placeholder 
                wait(1.0f);
                while(u) {
                    if (!kill_switch) {
                        u = false;
                        ledred = true;
                        wait(1.0f);
                        CurrentState = PositionCalibration;
                        pc.printf("current state = PositionCalibration\n\r");
                    }
                }
            break;  
        }
        wait(0.2f);         
    }
}