main.cpp

00001 //
00002 // COM_test : program to communicate via a COM port to a PC/laptop
00003 // ========
00004 //
00005 // Description
00006 //      Program to receive ASCII format commands through a virtual COM port and
00007 //      after executing the command return some data (optional, depending on
00008 //      the command) and a status value.
00009 //      The program understands two possible commands :
00010 //          "s i j"     : move servo 'i (0 to 5) to postion 'j' (0 to 90)
00011 //          "r"         : read the 'i' parameter of the last "s" command
00012 //
00013 // Version : 1.0
00014 // Author : Jim Herd
00015 // Date : 5th Oct 2011
00016 //
00017 #include "mbed.h"
00018 #include "MCP23017.h"
00019 #include "WattBob_TextLCD.h"
00020 #include "cmd_io.h"
00021 #include "globals.h"
00022 
00023 
00024 
00025 //******************************************************************************
00026 //declare ticker
00027 //
00028 Ticker timer1p;
00029 Ticker timer2p;
00030 Ticker timercounter1p;
00031 Ticker timercounter2p;
00032 Ticker timerstatemachine;
00033 
00034 //******************************************************************************
00035 //declare Pin
00036 //
00037 DigitalIn sensor1(p8);
00038 DigitalIn sensor2(p7);
00039 DigitalOut valueLED1(p23);
00040 DigitalOut valueLED2(p25);
00041 
00042 DigitalOut Reset(p27);
00043 
00044 DigitalOut led1(LED1);
00045 DigitalOut led2(LED2);
00046 DigitalOut led3(LED3);
00047 DigitalOut led4(LED4);
00048 
00049 DigitalOut clk(p24);
00050 
00051 DigitalIn counter1p(p5);
00052 DigitalIn counter2p(p6);
00053 DigitalIn SW1p(p11);
00054 DigitalIn SW2p(p12);
00055 
00056 //******************************************************************************
00057 //declare variable
00058 //
00059 bool Position1_1p;
00060 bool Position2_1p;
00061 bool Position1_2p;
00062 bool Position2_2p;
00063 bool Motor;
00064 
00065 bool bSort_Mode; //to use in the code to define the mode
00066 bool bMaint_Mode;//to use in the code to define the mode
00067 
00068 int state;
00069 
00070 
00071 
00072 //******************************************************************************
00073 // declare functions 
00074 //
00075 void sensor1p (void);
00076 void sensor2p (void);
00077 void counter1 (void);
00078 void counter2 (void);
00079 void state_machine (void);
00080 //    
00081 //
00082 // 2. five servo outputs
00083 //
00084 
00085 PwmOut servo_0(p26);
00086 //PwmOut servo_1(p25);
00087 //PwmOut servo_2(p24);
00088 //PwmOut servo_3(p23);
00089 PwmOut servo_4(p22);
00090 PwmOut servo_5(p21);
00091 
00092 //
00093 // 3. objects necessary to use the 2*16 character MBED display
00094 //
00095 MCP23017            *par_port;
00096 WattBob_TextLCD     *lcd;
00097 //
00098 // 4. Virtual COM port over USB link to laptop/PC
00099 //
00100 Serial      pc(USBTX, USBRX);
00101 
00102 //******************************************************************************
00103 // Defined GLOBAL variables and structures
00104 //
00105 CMD_STRUCT         ext_cmd;     // structure to hold command data
00106 STAT_STRUCT        ext_stat;    // structure to hold status reply
00107 
00108 
00109 //CMD_STRUCT commandcounter2;
00110 
00111 uint32_t   last_servo;          // store for last servo number
00112 
00113 //************************************************************************
00114 //************************************************************************
00115 // init_sys : initialise the system
00116 // ========
00117 //
00118 // 1. Configure 2*16 character display
00119 // 2. Print "COM test" string
00120 // 3. initialise relevant global variables
00121 // 4. set COM port baud rate to 19200 bits per second
00122 //
00123 void init_sys(void) {
00124 
00125     par_port = new MCP23017(p9, p10, 0x40);
00126     lcd = new WattBob_TextLCD(par_port);
00127 
00128     par_port->write_bit(1,BL_BIT);   // turn LCD backlight ON
00129     lcd->cls();
00130     lcd->locate(0,0);
00131     lcd->printf("W3C");
00132     
00133     servo_0.period(0.020);    // servo requires a 20ms period, common for all 5 servo objects
00134     last_servo = SERVO_UNKNOWN; 
00135     pc.baud(19200);
00136     
00137     servo_0.pulsewidth_us(1000 + (0 * 1000) / 90);
00138     servo_5.pulsewidth_us(1000 + (0 * 1000) / 90); 
00139     servo_4.pulsewidth_us(0);
00140 
00141     Position1_1p = 1;
00142     Position2_1p = 0;
00143     Position1_2p = 1;
00144     Position2_2p = 0;
00145     
00146     state = 0;
00147     Reset = 1;
00148     
00149     valueLED1=0;
00150     valueLED2=0;
00151     clk=0;
00152     return;
00153 }           // end init_sys
00154 
00155 //************************************************************************
00156 // process_cmd : decode and execute command
00157 // ===========
00158 uint32_t process_cmd(CMD_STRUCT *command)
00159 {
00160 int32_t pulse_width;
00161 
00162     switch (command->cmd_code) {
00163 //
00164 // move a servo
00165 //
00166         case SERVO_CMD :
00167             command->nos_data = 0;                   // no data to be returned
00168     //
00169     // check that parameters are OK
00170     //
00171             if (command->nos_params != 2) {          // check for 2 parameters
00172                 command->result_status = CMD_BAD_NUMBER_OF_PARAMETERS;
00173                 break;
00174             }
00175             if (command->param[0] > MAX_SERVO_NUMBER ) {       // check servo number
00176                 command->result_status = CMD_BAD_SERVO_NUMBER;
00177                 break;
00178             }
00179             if ((command->param[1] < MIN_SERVO_ANGLE) ||
00180                 (command->param[1] > MAX_SERVO_ANGLE) ) { 
00181                 command->result_status = CMD_BAD_SERVO_VALUE;
00182                 break;
00183             }
00184             if ((command->param[0] == 4) && (command->param[1] == 0)) {
00185                  pulse_width = 0;  // convert angle to pulse width
00186                  }
00187             else{ 
00188                 pulse_width = 1000 + (command->param[1] * 1000) / 90;  // convert angle to pulse width   
00189             }
00190              
00191             
00192             
00193     //
00194     // implement servo move to all 5 servos
00195     //
00196             switch (command->param[0]) {
00197                case 0 : servo_0.pulsewidth_us(pulse_width); break;
00198                case 4 : servo_4.pulsewidth_us(pulse_width); break;
00199                case 5 : servo_5.pulsewidth_us(pulse_width); break;
00200                 
00201             }
00202             last_servo = command->param[0];
00203             break;
00204 //
00205 // return last servo number                                                
00206 //
00207         case READ_CMD :
00208             
00209             if((bSort_Mode == 0)&&(bMaint_Mode == 1)){  
00210                 command->nos_data = 2;// no data to be returned                 
00211                 command->result_data[0] = valueLED1;
00212                 command->result_data[1] = valueLED2;
00213             }
00214             else if((bSort_Mode == 1)&&(bMaint_Mode == 0)){  
00215                 command->nos_data = 2;// no data to be returned                 
00216                 command->result_data[0] = valueLED1;// counter1p; 
00217                 command->result_data[1] = valueLED2;// counter2p; 
00218                /* command->result_data[2] = Motor;
00219                 command->result_data[3] = Position1_1p;
00220                 command->result_data[4] = Position2_1p;
00221                 command->result_data[5] = Position1_2p;
00222                 command->result_data[6] = Position2_2p;*/
00223                 //send_data(&ext_cmd);
00224             }
00225             break;
00226             
00227 //
00228 // Mode value                                               
00229 //
00230         case MAINT_MODE :
00231             bSort_Mode = 0;
00232             bMaint_Mode = 1;
00233             Reset = 1;
00234             servo_4.pulsewidth_us(0); 
00235             servo_0.pulsewidth_us(0);
00236             servo_5.pulsewidth_us(0); 
00237             lcd->cls();
00238             lcd->locate(0,0);
00239             lcd->printf("W3C");
00240             lcd->locate(1,0);
00241             lcd->printf("maintenance mode");
00242             break;
00243             
00244             
00245         case SORT_MODE :
00246             bSort_Mode = 1;
00247             bMaint_Mode = 0;
00248             Reset = 0;
00249             state = 0;
00250             lcd->cls(); 
00251             lcd->locate(0,0);
00252             lcd->printf("W3C");
00253             lcd->locate(1,0);
00254             lcd->printf("sort mode");
00255             
00256             break;
00257             
00258 //
00259 // Urgency mode
00260 //            
00261         case URGENCY :
00262             Reset = 1;
00263             bSort_Mode = 0;
00264             bMaint_Mode = 0;
00265             state = 0;
00266             servo_4.pulsewidth_us(0); 
00267             servo_0.pulsewidth_us(0);
00268             servo_5.pulsewidth_us(0); 
00269             lcd->cls();
00270             lcd->locate(0,0);
00271             lcd->printf("W3C");
00272             lcd->locate(1,0);
00273             lcd->printf("urgency mode");
00274             break;
00275 //
00276 // Exit mode
00277 // 
00278         case EXIT :
00279             Reset = 1;
00280             bSort_Mode = 0;
00281             bMaint_Mode = 0;
00282             state = 0;
00283             servo_4.pulsewidth_us(0); 
00284             servo_0.pulsewidth_us(0);
00285             servo_5.pulsewidth_us(0); 
00286             lcd->cls();
00287             lcd->locate(0,0);
00288             lcd->printf("W3C");
00289             break;     
00290 //
00291 // catch any problems
00292 //            
00293         default:
00294             command->nos_data = 0;                   // no data to be returned
00295             command->result_status = CMD_BAD_SERVO_VALUE;
00296             break;
00297     }
00298     return  OK;
00299 }
00300 
00301 //************************************************************************
00302 
00303 //function to send value on the FPGA when 1p is detected
00304 void sensor1p (void){
00305     sensor1.read();
00306     sensor2.read();
00307     
00308     clk = !clk;
00309     wait(0.01);
00310    
00311         if(sensor1 > 0.5) {
00312                 led1 = 1;
00313                 //if((bSort_Mode == 1)&&(bMaint_Mode == 0)){
00314                     valueLED1 = 1;
00315                 //}
00316         }
00317         else if(sensor1 < 0.5){
00318                led1 = 0;
00319                //if((bSort_Mode == 1)&&(bMaint_Mode == 0)){
00320                     valueLED1 = 0;
00321                //}
00322         }
00323 
00324         if(sensor2 > 0.5) {
00325                 led2 = 1;
00326                 valueLED2 = 1;
00327         }
00328         else if(sensor2 < 0.5){
00329                led2 = 0;
00330                 valueLED2 = 0;
00331         }
00332         
00333 }
00334 
00335 //function to send value on the FPGA when 2p is detected
00336 void sensor2p (){
00337     sensor2.read();
00338         if(sensor2 > 0.5) {
00339                 led2 = 1;
00340                 valueLED2 = 1;
00341         }
00342         else if(sensor2 < 0.5){
00343                led2 = 0;
00344                 valueLED2 = 0;
00345         }
00346 }
00347 
00348 
00349 //function for the state machine to move servos between 2 positions
00350 void state_machine (){
00351     if((bSort_Mode == 1)&&(bMaint_Mode == 0)){
00352     switch(state)
00353     {
00354         case 0:                                                  // initial state
00355              servo_4.pulsewidth_us(1000 + (25 * 1000) / 90);     // motor is run
00356              servo_0.pulsewidth_us(1000 + (0 * 1000) / 90);      // servo 1p go to position 1
00357              servo_5.pulsewidth_us(1000 + (0 * 1000) / 90);      // servo 2p go to position 1 
00358              Position1_1p = 1;
00359              Position2_1p = 0;
00360              Position1_2p = 1;
00361              Position2_2p = 0;
00362              Motor = 1;
00363              led3 = 0;
00364              led4 = 0;
00365              counter1p.read();
00366              counter2p.read();
00367              if(SW1p == 0){
00368                  if(counter1p > 0.5){
00369                     state = 1;
00370                  }
00371              }
00372              if(SW2p == 0){
00373                  if(counter2p > 0.5){
00374                     state = 4;
00375                  }
00376              }
00377              break;
00378         case 1:                                                 // state 1 if counter1p = 1
00379              servo_4.pulsewidth_us(0);                          // motor stop
00380              wait(2);
00381              servo_0.pulsewidth_us(1000 + (200 * 1000) / 90);   // servo 1p go to position 2 
00382              wait(1);
00383              Position1_1p = 0;
00384              Position2_1p = 1;
00385              Motor = 0;
00386              if((Position2_1p == 1)&&(counter1p < 0.5)){
00387                 state = 2;
00388              }
00389              break;
00390         case 2:                                                  // state 2 if servo 1p is in position 2
00391              servo_4.pulsewidth_us(1000 + (25 * 1000) / 90);     // motor is run   
00392              Motor = 1;
00393              counter1p.read();
00394              if(counter1p > 0.5){
00395                 state = 3;
00396              }
00397              break;
00398         case 3:                               // state 3 if counter 1p = 1
00399              servo_4.pulsewidth_us(0);        // motor stop
00400              Motor = 0;
00401              led3 = 1; 
00402              if(SW1p == 1){                   // wait SW 1p to go to the initial state
00403                 state = 0;
00404              }
00405              break;
00406         case 4:                                                // state 4 if counter 2p = 1
00407              servo_4.pulsewidth_us(0);                         // motor stop
00408              wait(2);
00409              servo_5.pulsewidth_us(1000 + (200 * 1000) / 90);  // servo 2p go to position 2
00410              wait(1);
00411              Position1_2p = 0;
00412              Position2_2p = 1;
00413              Motor = 0;
00414              if((Position2_2p == 1)&&(counter2p < 0.5)){
00415                 state = 5;
00416              }
00417              break;
00418         case 5:                                               // state 5 if servo 2p is in position 2
00419              servo_4.pulsewidth_us(1000 + (25 * 1000) / 90);  // motor run
00420              Motor = 1;
00421              counter2p.read();
00422              if(counter2p > 0.5){
00423                 state = 6;
00424              }
00425              break;
00426         case 6:                          // state 6 if counter 2p = 1
00427              servo_4.pulsewidth_us(0);   // motor stop
00428              Motor = 0;
00429              led4 = 1;
00430              if(SW2p == 1){              // wait SW 2p to go to the initial state
00431                     state = 0;
00432              }
00433              break;         
00434     } 
00435    } 
00436 }
00437 
00438 
00439 //************************************************************************
00440 //
00441 int main() {
00442     
00443     init_sys();
00444     Reset = 0;
00445     
00446     FOREVER {
00447     
00448      timer1p.attach(&sensor1p, 0.02);         //function sensor1p is reading all the 30 ms
00449    //  timer2p.attach(&sensor2p, 0.05);         //function sensor2p is reading all the 100 ms
00450     
00451     
00452      
00453         counter1p.read();
00454         counter2p.read();
00455         
00456         timerstatemachine.attach(&state_machine, 0.1);
00457    
00458     
00459 
00460      clk = !clk;
00461      wait(0.001);
00462     
00463         get_cmd(&ext_cmd);
00464     //
00465     // Check status of read command activity and return an error status if there was a problem
00466     // If there is a problem, then return status code only and wait for next command.
00467     //
00468         if (ext_cmd.result_status != OK){
00469             send_status(ext_cmd.result_status);
00470             continue;
00471         }
00472     //
00473     // Parse command and return an error staus if there is a problem
00474     // If there is a problem, then return status code only and wait for next command.
00475     //
00476         parse_cmd(&ext_cmd);
00477      //   lcd->locate(1,0); lcd->printf(ext_cmd.cmd_str);
00478         if ((ext_cmd.result_status != OK) && (ext_cmd.cmd_code != TEXT_CMD)){
00479              lcd->cls(); lcd->locate(0,0); lcd->printf("W3C"); lcd->locate(1,0); lcd->printf("parse : error");
00480             send_status(ext_cmd.result_status);
00481             continue;
00482         }
00483     //
00484     // Execute command and return an error staus if there is a problem
00485     //
00486         process_cmd(&ext_cmd);
00487         reply_to_cmd(&ext_cmd);
00488        
00489         
00490        
00491     
00492     }
00493          
00494 }
00495 
00496 
00497 
00498 
00499 
00500 
00501 
00502 
00503