File content as of revision 5:52badb8ee317:

#include "mbed.h"
#include "encoder.h"

PwmOut RightServoVel(D10);
PwmOut LeftServoVel(D11);

PwmOut      TurningMotorVel(D4);
DigitalOut  TurningMotorDir(D5);

Ticker ServoTick;
Ticker TurningTick;

DigitalOut ledr(LED_RED);
DigitalOut ledg(LED_GREEN);
DigitalOut ledb(LED_BLUE);

DigitalIn CcwTurn(D0);
DigitalIn CwTurn(D1);
InterruptIn ServoSwitch(D2);
DigitalIn qGreenPos(D6);                //Uitzoeken of we de Mbed kunnen linken

int maxPulses = 6000;
int halfPulses = maxPulses/2;
int backPulses = maxPulses*4/7 ;
bool ToTurnOrNotToTurn;

Encoder encoder3(D13,D12);

bool CcwRot = true;
bool CwRot  = !CcwRot;

enum TurningStates{CcwTurning,CwTurning,NoTurning};
enum ServoStates{Open, Close};

TurningStates currentState_T = NoTurning;

ServoStates currentState_S = Close;
ServoStates previousState_S = Close;

int j = 0;
int count = 0;

void ProcessStateMachineTurning(){
    ToTurnOrNotToTurn = (qGreenPos) ? true : false;        //Determening if the arm goes underneath the base
        if(ToTurnOrNotToTurn){
            if(CcwTurn && CwTurn){
                currentState_T = NoTurning;                            //No turning when both buttons are pressed
            } else if(CcwTurn){
                currentState_T = CcwTurning;                           //Turning Ccw
            } else if (CwTurn){
                currentState_T = CwTurning;                             //Turning Cw
            } else {
                currentState_T = NoTurning;                            //No buttons pressed No turning
            }
        } 
        else {
            currentState_T = NoTurning;
        }
    
    switch (currentState_T){
        case CcwTurning:
            if(encoder3.getPosition() > 0) {                        //Turning Counter clock wise
                TurningMotorVel.write(0.1f);
                TurningMotorDir.write(CcwRot);
            }
            else{
                while(encoder3.getPosition() < (backPulses)){
                    TurningMotorVel.write(0.1f);                    //Turning Back in Cw 
                    TurningMotorDir.write(CwRot);
                }
            }
        break;
        
        case CwTurning:
            if (encoder3.getPosition() < maxPulses){                //Turning clock wise
                TurningMotorVel.write(0.1f);                    //Turning  in Cw 
                TurningMotorDir.write(CwRot);
            }
            else{
                while(encoder3.getPosition() > (maxPulses - backPulses)){
                    TurningMotorVel.write(0.1f);                    //Turning Back in Ccw 
                    TurningMotorDir.write(CcwRot);
                }
            }
        break;
        
        case NoTurning:
            TurningMotorVel.write(0.0f);
        break;
    }
}


void ProcessStateMachineServo(void) {
/**
State machine for the servo motors, motor response dependent on the current state and the amount of milliseconds of activation.
The entire function must be sampled at 2000 Hz, since it counts half milliseconds. This is required since the servos respond to half millisecond actuation.
For further information on the functioning of the servo motors; see paragraph ... in report 'Design of a Jenga playing robot' by group 2.
**/
    switch (currentState_S)
        {        
        case Close:
            if(j <= 35){
                RightServoVel.write(0); LeftServoVel.write(0);
                j++;
            }
            else if (j == 36){
                RightServoVel.write(1); LeftServoVel.write(0);
                j++;
            }
            else if (j == 37){
                RightServoVel.write(1); LeftServoVel.write(0);
                j++;
            }
            else if (j == 38){
                RightServoVel.write(1); LeftServoVel.write(0);
                j++;
            }
            else if (j == 39){
                RightServoVel.write(1); LeftServoVel.write(1);
                j = 0;
            }
            previousState_S = Close;
        break;
        
        case Open:
            if(j <= 35){
                RightServoVel.write(0); LeftServoVel.write(0);
                j++;
            }
            else if (j == 36){
                RightServoVel.write(0); LeftServoVel.write(0);
                j++;
            }
            else if (j == 37){
                RightServoVel.write(0); LeftServoVel.write(1);
                j++;
            }
            else if (j == 38){
                RightServoVel.write(1); LeftServoVel.write(1);
                j++;
            }
            else if (j == 39){
                RightServoVel.write(1); LeftServoVel.write(1);
                j = 0;
            }
            previousState_S = Open;
        break;
    }
}

void ChangingState(){
    currentState_S = previousState_S;
}
    

int main()
{
    ledr = 1;
    ledg = 1;
    ledb = 1;
    
    ServoSwitch.rise(&ChangingState);
    
    ServoTick.attach(ProcessStateMachineServo,0.0005f);
    TurningTick.attach(ProcessStateMachineTurning, 0.001f);
    
    encoder3.setPosition(halfPulses);
        
    while (true){
        if (!CcwTurn){
            ledr = 0; ledg = 1; ledb = 1;    // ROOD
        } else if(!CwTurn){
            ledr = 1; ledg = 0; ledb = 1;    //GROEN
        } else if(!ServoSwitch){
            ledr = 1; ledg = 1; ledb = 0;                      //BLAUW
        } else {
            ledr = 1; ledg = 1; ledb = 1;           
        }      
    }
}