File content as of revision 0:e5b9c99f23f2:

//
// COM_test : program to communicate via a COM port to a PC/laptop
// ========
//
// Description
//      Program to receive ASCII format commands through a virtual COM port and
//      after executing the command return some data (optional, depending on
//      the command) and a status value.
//      The program understands two possible commands :
//          "s i j"     : move servo 'i (0 to 5) to postion 'j' (0 to 90)
//          "r"         : read the 'i' parameter of the last "s" command
//
// Version : 1.0
// Author : Jim Herd
// Date : 5th Oct 2011
//
#include "mbed.h"
#include "MCP23017.h"
#include "WattBob_TextLCD.h"
#include "cmd_io.h"
#include "globals.h"



Ticker timer1p;
Ticker timer2p;
Ticker timercounter1p;
Ticker timercounter2p;


AnalogIn sensor1(p15);
AnalogIn sensor2(p16);
AnalogOut valueLED1(p18);
DigitalOut valueLED2(p25);


DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

DigitalOut clk(p24);

AnalogIn counter1p(p19);
AnalogIn counter2p(p20);





//******************************************************************************
// declare functions 
//
void sensor1p (void);
void sensor2p (void);
void counter1 (void);
void counter2 (void);

//    
//
// 2. five servo outputs
//

PwmOut servo_0(p26);
//PwmOut servo_1(p25);
//PwmOut servo_2(p24);
//PwmOut servo_3(p23);
PwmOut servo_4(p22);
PwmOut servo_5(p21);

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

//******************************************************************************
// Defined GLOBAL variables and structures
//
CMD_STRUCT         ext_cmd, commandcounter2;     // 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,0);
    lcd->printf("COM ");
    
    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_4.pulsewidth_us(1000 + (25 * 1000) / 90);
    servo_5.pulsewidth_us(1000 + (0 * 1000) / 90); 

    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)) {
                 pulse_width = 0;  // convert angle to pulse width
                 }
            else{ 
                pulse_width = 1000 + (command->param[1] * 1000) / MAX_SERVO_ANGLE;  // convert angle to pulse width   
            }
             
            
            
    //
    // implement servo move to all 5 servos
    //
            switch (command->param[0]) {
               case 0 : servo_0.pulsewidth_us(pulse_width); break;
           //     case 1 : servo_1.pulsewidth_us(pulse_width); break;
          //      case 2 : servo_2.pulsewidth_us(pulse_width); break;
           //     case 3 : servo_3.pulsewidth_us(pulse_width); break;
               case 4 : servo_4.pulsewidth_us(pulse_width); break;
           //     case 5 : servo_5.pulsewidth_us(pulse_width); break;
                
            }
            last_servo = command->param[0];
            break;
//
// return last servo number
//
        case READ_CMD :
            command->nos_data = 2;                   // no data to be returned
            command->result_data[0] = valueLED1;
            command->result_data[1] = valueLED2;
            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 is detected
void sensor1p (void){
    clk = !clk;
    wait(0.01);
    sensor1.read();
        if(sensor1 > 0.5) {
                led1 = 1;
                valueLED1 = 1;
        }
        else if(sensor1 < 0.5){
               led1 = 0;
                valueLED1 = 0;
        }
}

//function to send value on the FPGA when 2p is detected
void sensor2p (){
    sensor2.read();
        if(sensor2 > 0.5) {
                led2 = 1;
                valueLED2 = 1;
        }
        else if(sensor2 < 0.5){
               led2 = 0;
                valueLED2 = 0;
        }
}

//function to move servo when we have counter1p equal to 10
void counter1 (){
    if(counter1p > 0.5){
            led3 = 1;
            servo_4.pulsewidth_us(0);
            wait(1);
            servo_0.pulsewidth_us(1000 + (200 * 1000) / 90); 
         }
         else{
            led3 = 0;
            servo_0.pulsewidth_us(1000 + (0 * 1000) / 90); 
            if(counter2p < 0.5){
            servo_4.pulsewidth_us(1000 + (25 * 1000) / 90);
            }
         }
}

//function to move servo when we have counter2p equal to 5
void counter2(){
    if(counter2p > 0.5){
            led4 = 1;
            servo_4.pulsewidth_us(0);
            wait(1);
            servo_5.pulsewidth_us(1000 + (200 * 1000) / 90);    
         }
         else{
            led4 = 0;
            servo_5.pulsewidth_us(1000 + (0 * 1000) / 90); 
            if(counter1p < 0.5){
            servo_4.pulsewidth_us(1000 + (25 * 1000) / 90);
            }
         }
}




//************************************************************************
//
int main() {

    valueLED1=0;
    valueLED2=0;
    clk=0;
    init_sys();
    
    FOREVER {
    
    timer2p.attach(&sensor2p, 0.1);         //function sensor2p is reading all the 0.1 ms
    timer1p.attach(&sensor1p, 0.1);         //function sensor1p is reading all the 0.1 ms
    timercounter2p.attach(&counter2, 0.1);  //function counter2 is reading all the 0.1 ms
    timercounter1p.attach(&counter1, 0.1);  //function counter1 is reading all the 0.1 ms
    
     clk = !clk;
     wait(0.01);
    
        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->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);
        
       
    
    }
         
}