File content as of revision 2:881ca0a50c9b:

#include "mbed.h"
#include "MCP23017.h"
#include "WattBob_TextLCD.h"
#include "cmd_io.h"
#include "globals.h"


//******************************************************************************
//declare ticker
//
Ticker timersensor;             // Timer for the function of the sensor 
Ticker timerstatemachine;       // Timer for the function of the state machine

//******************************************************************************
//declare Pin
//
DigitalIn sensor1(p8);                  // sensor which detect the coin of 2p
DigitalIn sensor2(p7);                  // sensor which detect the coin of 1p
DigitalIn counter1p(p5);                // input to know when the counter 1p = 10
DigitalIn counter2p(p6);                // input to know when the counter 2p = 5
DigitalIn SW1p(p11);                    // Switch for the 1p
DigitalIn SW2p(p12);                    // Switch for the 2p

DigitalOut valueLED1(p23);              // output sensor 2p send to the FPGA
DigitalOut valueLED2(p25);              // output sensor 1p send to the FPGA
DigitalOut Reset(p27);                  // variable send to the FPGA to say when we should reset the counter
DigitalOut led1(LED1);                  // declaration of the led1 (sensor 2p)
DigitalOut led2(LED2);                  // declaration of the led2 (sensor 1p)
DigitalOut led3(LED3);                  // declaration of the led3 (when counter 1p = 10)
DigitalOut led4(LED4);                  // declaration of the led4 (when counter 2p = 5)
DigitalOut clk(p24);                    // clock send to the FPGA



//******************************************************************************
//declare variable
//
bool Position1_1p;                      // Position 1 for the 1p 
bool Position2_1p;                      // Position 2 for the 1p
bool Position1_2p;                      // Position 1 for the 2p
bool Position2_2p;                      // Position 2 for the 2p
bool Motor;                             // variable send to the GUI to know when the motor run

bool bSort_Mode;                        // to use in the code to define the mode
bool bMaint_Mode;                       // to use in the code to define the mode

int state;                              // declaration of the variable state for the state machine


//******************************************************************************
// declare functions 
//
void sensor (void);               // Function to read sensors
void state_machine (void);        // Function to go in the state machine to move the servos and the motor 

//
// 3 servo outputs
//

PwmOut servo_0(p26);            // servo to move the boxes of 1p
PwmOut servo_4(p22);            // motor of the wheel
PwmOut servo_5(p21);            // servo to move the boxes of 2p

//
// objects necessary to use the 2*16 character MBED display
//
MCP23017            *par_port;
WattBob_TextLCD     *lcd;
//
//  Virtual COM port over USB link to laptop/PC
//
Serial      pc(USBTX, USBRX);

//******************************************************************************
// Defined GLOBAL variables and structures
//
CMD_STRUCT         ext_cmd;     // structure to hold command data
STAT_STRUCT        ext_stat;    // structure to hold status reply


//CMD_STRUCT commandcounter2;

uint32_t   last_servo;          // store for last servo number

//************************************************************************
//************************************************************************
// init_sys : initialise the system
// ========
//
// 1. Configure 2*16 character display
// 2. Print "COM test" string
// 3. initialise relevant global variables
// 4. set COM port baud rate to 19200 bits per second
//
void init_sys(void) {

    par_port = new MCP23017(p9, p10, 0x40);
    lcd = new WattBob_TextLCD(par_port);

    par_port->write_bit(1,BL_BIT);   // turn LCD backlight ON
    lcd->cls();
    lcd->locate(0,7);
    lcd->printf("W3C");
    
    servo_0.period(0.020);    // servo requires a 20ms period, common for all 5 servo objects
    last_servo = SERVO_UNKNOWN; 
    pc.baud(19200);
    
    servo_0.pulsewidth_us(1000 + (0 * 1000) / 90);  // Servo 1p initialise in position 1
    servo_5.pulsewidth_us(1000 + (0 * 1000) / 90);  // Servo 2p initialise in position 1
    servo_4.pulsewidth_us(0);                       // Motor stop

    Position1_1p = 1;       // servo 1p is in position 1                         
    Position2_1p = 0;       // position 2 = 0                             
    Position1_2p = 1;       // servo 2p is in position 1                          
    Position2_2p = 0;       // position 2 = 0                    
    
    state = 10;             // initial state is state 10
    Reset = 1;              // reset the counter in the FPGA
    
    valueLED1 = 0;          // sensor 2p initialize to 0
    valueLED2 = 0;          // sensor 1p initialize to 0
    
    clk = 0;                // clock start to 0   
    Motor = 0;              // motor is stop
    return;
}           // end init_sys

//************************************************************************
// process_cmd : decode and execute command
// ===========
uint32_t process_cmd(CMD_STRUCT *command)
{
int32_t pulse_width;

    switch (command->cmd_code) {
//
// move a servo
//
        case SERVO_CMD :
            command->nos_data = 0;                   // no data to be returned
    //
    // check that parameters are OK
    //
            if (command->nos_params != 2) {          // check for 2 parameters
                command->result_status = CMD_BAD_NUMBER_OF_PARAMETERS;
                break;
            }
            if (command->param[0] > MAX_SERVO_NUMBER ) {       // check servo number
                command->result_status = CMD_BAD_SERVO_NUMBER;
                break;
            }
            if ((command->param[1] < MIN_SERVO_ANGLE) ||
                (command->param[1] > MAX_SERVO_ANGLE) ) { 
                command->result_status = CMD_BAD_SERVO_VALUE;
                break;
            }
            if ((command->param[0] == 4) && (command->param[1] == 0)) {     // motor of the wheel
                 pulse_width = 0;                                           // pulse width is 0 so motor stop
                 }
            else{ 
                pulse_width = 1000 + (command->param[1] * 1000) / 90;       // convert angle to pulse width   
            }
             
            
            
    //
    // implement servo move to all 3 servos
    //
            switch (command->param[0]) {
               case 0 : servo_0.pulsewidth_us(pulse_width); break;          // servo for 1p
               case 4 : servo_4.pulsewidth_us(pulse_width); break;          // motor
               case 5 : servo_5.pulsewidth_us(pulse_width); break;          // servo for 2p
            }
            last_servo = command->param[0];
            break;
//
// return data of the value of sensor 1p and sensor 2p                                                
//
        case READ_CMD :
            if((bSort_Mode == 0)&&(bMaint_Mode == 1)){      // when we are on sort mode  
                command->nos_data = 2;               
                command->result_data[0] = valueLED1;        // return data of sensor 2p to the GUI
                command->result_data[1] = valueLED2;        // return data of sensor 1p to the GUI
            }
            break;
            
//
// Maintenance Mode                                               
//
        case MAINT_MODE :                                  
            bSort_Mode = 0;
            bMaint_Mode = 1;
            Reset = 1;                          // we reset the counter in the FPGA
            servo_4.pulsewidth_us(0);           // Motor stop 
            servo_0.pulsewidth_us(0);           // Servo 1p stop
            servo_5.pulsewidth_us(0);           // Servo 2p stop
            lcd->cls();
            lcd->locate(0,7);
            lcd->printf("W3C");                 // write on the LCD
            lcd->locate(1,0);
            lcd->printf("maintenance mode");
            break;
            
//
// Sort Mode
//            
        case SORT_MODE :                                    
            bSort_Mode = 1;
            bMaint_Mode = 0;               
            Reset = 0;                      
            state = 10;                 // state in the state machine is 10
            lcd->cls(); 
            lcd->locate(0,7);
            lcd->printf("W3C");         //  write on the LCD
            lcd->locate(1,0);
            lcd->printf("sort mode");      
            break;
            
//
// Urgency mode
//            
        case URGENCY :                                    
            Reset = 1;                  // we reset the counter on the FPGA
            bSort_Mode = 0;
            bMaint_Mode = 0;
            state = 10;                 // state in the state machine is 10
            servo_4.pulsewidth_us(0);   // Motor stop
            servo_0.pulsewidth_us(0);   // Servo 1p stop 
            servo_5.pulsewidth_us(0);   // Servo 2p stop
            lcd->cls();
            lcd->locate(0,7);
            lcd->printf("W3C");         // write on the LCD
            lcd->locate(1,0);
            lcd->printf("urgency mode");
            break;
//
// Exit mode
// 
        case EXIT :
            Reset = 1;                      // we reset the counter in the FPGA
            bSort_Mode = 0;
            bMaint_Mode = 0;
            state = 10;                     // state in the state machine is 10
            servo_4.pulsewidth_us(0);       // Motor stop
            servo_0.pulsewidth_us(0);       // sensor 1p stop
            servo_5.pulsewidth_us(0);       // sensor 2p stop
            lcd->cls();
            lcd->locate(0,7);
            lcd->printf("W3C");             // write on the LCD
            break;   

// Send data of the value led 2p
        case VALUE_LED1 :
            command->nos_data = 1;
            command->result_data[0] = valueLED1;        // return data of sensor 2p to the GUI
            break;

// Send data of the value led 1p        
        case VALUE_LED2 :
            command->nos_data = 2;
            command->result_data[1] = valueLED2;        // return data of sensor 1p to the GUI
            break; 

// Send data of the value counter 1p        
        case COUNTER1P :
            command->nos_data = 3;
            command->result_data[2] = counter1p;        // return data of counter 1p to the GUI (counter 1p = 1 when we have 10 coins)
            break;

// Send data of the value counter 2p            
        case COUNTER2P :
            command->nos_data = 4;
            command->result_data[3] = counter2p;        // return data of counter 2p to the GUI (counter 2p = 1 when we have 5 coins)
            break;

// Send data of the value position1 1p            
        case POSITION1_1P :
            command->nos_data = 1;
            command->result_data[0] = Position1_1p;     // return data of the position 1 for the servo of the 1p to the GUI
            break;

// Send data of the value position1 2p       
       case POSITION1_2P :
            command->nos_data = 2;
            command->result_data[1] = Position1_2p;     // return data of the position 1 for the servo of the 2p to the GUI
            break;    

// Send data of the motor            
       case MOTOR :
            command->nos_data = 3;
            command->result_data[2] = Motor;            // return data of the state of motor to the GUI
            break; 
                       
//
// catch any problems
//            
        default:
            command->nos_data = 0;                   // no data to be returned
            command->result_status = CMD_BAD_SERVO_VALUE;
            break;
    }
    return  OK;
}

//************************************************************************

//function to send value on the FPGA when 1p or 2p are detected
void sensor (void){
    sensor1.read();                     // read input sensor 2p
    sensor2.read();                     // read input sensor 1p
    
    clk = !clk;                         // inverse signal of the clock
    wait(0.01);
   
        if(sensor1 > 0.5) {            // if coin of 2p is detected
               led1 = 1;               // led 1 switch on
               valueLED1 = 1;          // value of the output sensor 2p send to the FPGA
        }
        else if(sensor1 < 0.5){        // if coin of 2p is not detected
               led1 = 0;               // led 1 switch off
               valueLED1 = 0;          // value of the output sensor 2p send to the FPGA
        }
        if(sensor2 > 0.5) {            // if coin of 1p is detected
               led2 = 1;               // led 2 switch on
               valueLED2 = 1;          // value of the output sensor 1p send to the FPGA
        }
        else if(sensor2 < 0.5){        // of coin of 1p is not detected
               led2 = 0;               // led 2 switch off
               valueLED2 = 0;          // value of the output sensor 1p send to the FPGA
        }    
}

//function for the state machine to move servos between 2 positions and to move motor
void state_machine (){
    if((bSort_Mode == 1)&&(bMaint_Mode == 0)){      // if we are in sort mode

    switch(state)
    {
        case 10 :                                               // initial state 
             servo_4.pulsewidth_us(1000 + (25 * 1000) / 90);    // motor is run
             servo_0.pulsewidth_us(1000 + (0 * 1000) / 90);     // servo 1p go to position 1
             servo_5.pulsewidth_us(1000 + (0 * 1000) / 90);     // servo 2p go to position 1 
             Position1_1p = 1;                                  // value position 1 for servo 1p = 1
             Position2_1p = 0;
             Position1_2p = 1;                                  // value position 1 for servo 2p = 1
             Position2_2p = 0;
             Motor = 1;                                         // value to say motor running 
             led3 = 0;
             led4 = 0;
             state = 0;                                         // go to state 0
             break;
    
    
        case 0:                                                  // state 0
             Motor = 1;                                          // motor running
             led3 = 0;
             led4 = 0;
             counter1p.read();                                  // read value of counter 1p
             counter2p.read();                                  // read value of counter 2p
             if(SW1p == 0){                                     // if switch of 1p os switch off
                 if(counter1p > 0.5){                           // if counter 1p = 10 coins
                    state = 1;                                  // go to state 1
                 }
             }
             if(SW2p == 0){                                     // if switch 2p is switch off
                 if(counter2p > 0.5){                           // if counter 2p = 5 coins
                    state = 4;                                  // go to state 4
                 }
             }
             break;
        case 1:                                                 // state 1
             servo_4.pulsewidth_us(0);                          // motor stop
             wait(2);
             servo_0.pulsewidth_us(1000 + (200 * 1000) / 90);   // servo 1p go to position 2 
             wait(1);
             Position1_1p = 0;
             Position2_1p = 1;                                  // position 2 for servo 1p = 1
             Motor = 0;                                         // motor stopping
             if((Position2_1p == 1)&&(counter1p < 0.5)){        // if servo 1p is in position 2 and counter 1p < 10 coins
                state = 2;                                      // go to state 2
             }
             break;
        case 2:                                                  // state 2
             servo_4.pulsewidth_us(1000 + (25 * 1000) / 90);     // motor is run   
             Motor = 1;
             counter1p.read();                                  // read value of counter 1p
             counter2p.read();                                  // read value of counter 2p
             if(counter1p > 0.5){                               // if counter 1p = 10 coins
                state = 3;                                      // go to state 3
             }
             else if((counter2p > 0.5)&&(Position1_2p == 1)){   // if counter 2p = 5 coins and servo 2p is in position 1
                state = 4;                                      // go to state 4
             }
             else if((counter2p > 0.5)&&(Position2_2p == 1)){   // if counter 2p = 5 coins and servo 2p is in position 2
                state = 6;                                      // go to state 6
             }
             break;
        case 3:                                                  // state 3 
             servo_4.pulsewidth_us(0);                           // motor stop
             Motor = 0;
             led3 = 1;                                           // led 3 switch on because we have 2 boxes of 1p full
             if(SW1p == 1){                                      // if switch 1p is switched
                servo_0.pulsewidth_us(1000 + (0 * 1000) / 90);   // servo 1p go to position 1
                state = 0;                                       // go to state 0
             }
             break;
        case 4:                                                 // state 4 
             servo_4.pulsewidth_us(0);                          // motor stop
             wait(2);
             servo_5.pulsewidth_us(1000 + (200 * 1000) / 90);   // servo 2p go to position 2
             wait(1);
             Position1_2p = 0;
             Position2_2p = 1;                                  // position 2 for servo 2p = 1
             Motor = 0;                                         // motor stopping
             if((Position2_2p == 1)&&(counter2p < 0.5)){        // if servo 2p is in position 2 and counter 2p < 5 coins
                state = 5;                                      // go to state 5
             }
             break;
        case 5:                                                 // state 5
             servo_4.pulsewidth_us(1000 + (25 * 1000) / 90);    // motor run
             Motor = 1;                                     
             counter2p.read();                                  // read value of counter 2p
             counter1p.read();                                  // read value of counter 1p
             if(counter2p > 0.5){                               // if counter 2p = 5 coins
                state = 6;                                      // go to state 6
             }
             else if((counter1p > 0.5)&&(Position1_1p == 1)){   // if counter 1p = 10 coins and servo 1p is in position 1
                state = 0;                                      // go to state 0
             }
             else if((counter1p > 0.5)&&(Position2_1p == 1)){   // if counter 1p = 10 coins and servo 1p is in position 2
                state = 3;                                      // go to state 3
             }
             break;
        case 6:                                                     // state 6 
             servo_4.pulsewidth_us(0);                              // motor stop
             Motor = 0;
             led4 = 1;                                              // led 4 switch on because we have 2 boxes of 2p full
             if(SW2p == 1){                                         // if switch 2p is switched
                    servo_5.pulsewidth_us(1000 + (0 * 1000) / 90);  // servo 2p go to position 1
                    state = 0;                                      // go to state 0
             }
             break;         
    } 
   } 
}

//
//************************************************************************
// Main Program
//
int main() {
    
    init_sys();                                 // call function of init system
    Reset = 0;                                  // we not reset the counter in the FPGA
    
    FOREVER {
    
     timersensor.attach(&sensor, 0.02);         //function sensor is reading all the 20 ms 
     
        counter1p.read();                       // read value of counter 1p
        counter2p.read();                       // read value of coubter 2p
        
     timerstatemachine.attach(&state_machine, 0.1);     // function state machine is readinf all the 100 ms

     clk = !clk;                                // inverse signal of the clock
     wait(0.001);
    
    get_cmd(&ext_cmd);
    //
    // Check status of read command activity and return an error status if there was a problem
    // If there is a problem, then return status code only and wait for next command.
    //
        if (ext_cmd.result_status != OK){
            send_status(ext_cmd.result_status);
            continue;
        }
    //
    // Parse command and return an error staus if there is a problem
    // If there is a problem, then return status code only and wait for next command.
    //
        parse_cmd(&ext_cmd);
     //   lcd->locate(1,0); lcd->printf(ext_cmd.cmd_str);
        if ((ext_cmd.result_status != OK) && (ext_cmd.cmd_code != TEXT_CMD)){
             lcd->cls(); lcd->locate(0,0); lcd->printf("W3C"); lcd->locate(1,0); lcd->printf("parse : error");
            send_status(ext_cmd.result_status);
            continue;
        }
    //
    // Execute command and return an error staus if there is a problem
    //
        process_cmd(&ext_cmd);
        reply_to_cmd(&ext_cmd);
    }
         
}