Diff: main.cpp

Revision:

0:247c337c230a

Child:

1:8a1818e89c49

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Dec 01 11:35:54 2011 +0000
@@ -0,0 +1,507 @@
+#include "mbed.h"
+#include "MCP23017.h"
+#include "WattBob_TextLCD.h"
+#include "cmd_io.h"
+#include "globals.h"
+
+
+//******************************************************************************
+//declare ticker
+//
+Ticker timersensor;
+Ticker timerstatemachine;
+
+//******************************************************************************
+//declare Pin
+//
+DigitalIn sensor1(p8);
+DigitalIn sensor2(p7);
+DigitalIn counter1p(p5);                // Value when counter 1p = 10
+DigitalIn counter2p(p6);                // Value when counter 2p = 5
+DigitalIn SW1p(p11);                    // Switch for the 1p
+DigitalIn SW2p(p12);                    // Switch for the 2p
+
+DigitalOut valueLED1(p23);              // value sensor 2p send to the FPGA
+DigitalOut valueLED2(p25);              // value sensor 1p send to the FPGA
+DigitalOut Reset(p27);
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+DigitalOut clk(p24);
+
+
+
+//******************************************************************************
+//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;
+
+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;
+
+
+
+//******************************************************************************
+// 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);
+PwmOut servo_4(p22);
+PwmOut servo_5(p21);
+
+//
+// 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;                               
+    Position2_1p = 0;                               
+    Position1_2p = 1;                               
+    Position2_2p = 0;                               
+    
+    state = 10;
+    Reset = 1;
+    
+    valueLED1=0;
+    valueLED2=0;
+    
+    clk=0;
+    Motor = 0;
+    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) / 90;  // 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 4 : servo_4.pulsewidth_us(pulse_width); break;
+               case 5 : servo_5.pulsewidth_us(pulse_width); break;
+            }
+            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;
+                command->result_data[1] = valueLED2;
+            }
+            break;
+            
+//
+// Mode value                                               
+//
+        case MAINT_MODE :                                  // Maintenance mode
+            bSort_Mode = 0;
+            bMaint_Mode = 1;
+            Reset = 1;
+            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");
+            lcd->locate(1,0);
+            lcd->printf("maintenance mode");
+            break;
+            
+            
+        case SORT_MODE :                                    // sort mode
+            bSort_Mode = 1;
+            bMaint_Mode = 0;               
+            Reset = 0;
+            state = 10;
+            lcd->cls(); 
+            lcd->locate(0,7);
+            lcd->printf("W3C");
+            lcd->locate(1,0);
+            lcd->printf("sort mode");      
+            break;
+            
+//
+// Urgency mode
+//            
+        case URGENCY :                                    
+            Reset = 1;
+            bSort_Mode = 0;
+            bMaint_Mode = 0;
+            state = 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");
+            lcd->locate(1,0);
+            lcd->printf("urgency mode");
+            break;
+//
+// Exit mode
+// 
+        case EXIT :
+            Reset = 1;
+            bSort_Mode = 0;
+            bMaint_Mode = 0;
+            state = 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");
+            break;   
+
+// Send data of the value led 2p
+        case VALUE_LED1 :
+            command->nos_data = 1;
+            command->result_data[0] = valueLED1;
+            break;
+
+// Send data of the value led 1p        
+        case VALUE_LED2 :
+            command->nos_data = 2;
+            command->result_data[1] = valueLED2;
+            break; 
+
+// Send data of the value counter 1p        
+        case COUNTER1P :
+            command->nos_data = 3;
+            command->result_data[2] = counter1p;
+            break;
+
+// Send data of the value counter 2p            
+        case COUNTER2P :
+            command->nos_data = 4;
+            command->result_data[3] = counter2p;
+            break;
+
+// Send data of the value position1 1p            
+        case POSITION1_1P :
+            command->nos_data = 1;
+            command->result_data[0] = Position1_1p;
+            break;
+
+// Send data of the value position1 2p       
+       case POSITION1_2P :
+            command->nos_data = 2;
+            command->result_data[1] = Position1_2p;
+            break;    
+
+// Send data of the motor            
+       case MOTOR :
+            command->nos_data = 3;
+            command->result_data[2] = Motor;
+            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();
+    sensor2.read();
+    
+    clk = !clk;
+    wait(0.01);
+   
+        if(sensor1 > 0.5) {
+                led1 = 1;
+                    valueLED1 = 1;
+        }
+        else if(sensor1 < 0.5){
+               led1 = 0;
+                    valueLED1 = 0;
+        }
+        if(sensor2 > 0.5) {
+                led2 = 1;
+                valueLED2 = 1;
+        }
+        else if(sensor2 < 0.5){
+               led2 = 0;
+                valueLED2 = 0;
+        }    
+}
+
+
+//function for the state machine to move servos between 2 positions
+void state_machine (){
+    if((bSort_Mode == 1)&&(bMaint_Mode == 0)){
+    
+    
+    
+    switch(state)
+    {
+        case 10 :
+             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;
+             Position2_1p = 0;
+             Position1_2p = 1;
+             Position2_2p = 0;
+             Motor = 1;
+             led3 = 0;
+             led4 = 0;
+             state = 0;
+             break;
+    
+    
+        case 0:                                                  // initial state
+             Motor = 1;
+             led3 = 0;
+             led4 = 0;
+             counter1p.read();
+             counter2p.read();
+             if(SW1p == 0){
+                 if(counter1p > 0.5){
+                    state = 1;
+                 }
+             }
+             if(SW2p == 0){
+                 if(counter2p > 0.5){
+                    state = 4;
+                 }
+             }
+             break;
+        case 1:                                                 // state 1 if counter1p = 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;
+             Motor = 0;
+             if((Position2_1p == 1)&&(counter1p < 0.5)){
+                state = 2;
+             }
+             break;
+        case 2:                                                  // state 2 if servo 1p is in position 2
+             servo_4.pulsewidth_us(1000 + (25 * 1000) / 90);     // motor is run   
+             Motor = 1;
+             counter1p.read();
+             counter2p.read();
+             if(counter1p > 0.5){
+                state = 3;
+             }
+             else if((counter2p > 0.5)&&(Position1_2p == 1)){
+                state = 4;
+             }
+             else if((counter2p > 0.5)&&(Position2_2p == 1)){
+                state = 6; 
+             }
+             break;
+        case 3:                                                  // state 3 if counter 1p = 1
+             servo_4.pulsewidth_us(0);                           // motor stop
+             Motor = 0;
+             led3 = 1; 
+             if(SW1p == 1){                                      // wait SW 1p to go to the initial state
+                servo_0.pulsewidth_us(1000 + (0 * 1000) / 90);
+                state = 0;
+             }
+             break;
+        case 4:                                                // state 4 if counter 2p = 1
+             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;
+             Motor = 0;
+             if((Position2_2p == 1)&&(counter2p < 0.5)){
+                state = 5;
+             }
+             break;
+        case 5:                                               // state 5 if servo 2p is in position 2
+             servo_4.pulsewidth_us(1000 + (25 * 1000) / 90);  // motor run
+             Motor = 1;
+             counter2p.read();
+             counter1p.read();
+             if(counter2p > 0.5){
+                state = 6;
+             }
+             else if((counter1p > 0.5)&&(Position1_1p == 1)){
+                state = 0;
+             }
+             else if((counter1p > 0.5)&&(Position2_1p == 1)){
+                state = 3; 
+             }
+             break;
+        case 6:                                               // state 6 if counter 2p = 1
+             servo_4.pulsewidth_us(0);                        // motor stop
+             Motor = 0;
+             led4 = 1;
+             if(SW2p == 1){                                   // wait SW 2p to go to the initial state
+                    servo_5.pulsewidth_us(1000 + (0 * 1000) / 90);
+                    state = 0;
+             }
+             break;         
+    } 
+   } 
+}
+
+//
+//************************************************************************
+// Main Program
+//
+int main() {
+    
+    init_sys();
+    Reset = 0;
+    
+    FOREVER {
+    
+     timersensor.attach(&sensor, 0.02);         //function sensor is reading all the 20 ms 
+     
+        counter1p.read();
+        counter2p.read();
+        
+     timerstatemachine.attach(&state_machine, 0.1);     // function state machine is readinf all the 100 ms
+
+     clk = !clk;
+     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);
+    }
+         
+}
+
+
+
+
+
+
+
+
+