Nurbol Nurdaulet / Mbed 2 deprecated 2sensors_2servo_17_11_11
Committer:
Nurbol
Date:
Thu Nov 17 17:03:22 2011 +0000
Revision:
0:d33360d14a57

        

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Nurbol 0:d33360d14a57 1 //
Nurbol 0:d33360d14a57 2 // COM_test : program to communicate via a COM port to a PC/laptop
Nurbol 0:d33360d14a57 3 // ========
Nurbol 0:d33360d14a57 4 //
Nurbol 0:d33360d14a57 5 // Description
Nurbol 0:d33360d14a57 6 // Program to receive ASCII format commands through a virtual COM port and
Nurbol 0:d33360d14a57 7 // after executing the command return some data (optional, depending on
Nurbol 0:d33360d14a57 8 // the command) and a status value.
Nurbol 0:d33360d14a57 9 // The program understands two possible commands :
Nurbol 0:d33360d14a57 10 // "s i j" : move servo 'i (0 to 5) to postion 'j' (0 to 90)
Nurbol 0:d33360d14a57 11 // "r" : read the 'i' parameter of the last "s" command
Nurbol 0:d33360d14a57 12 //
Nurbol 0:d33360d14a57 13 // Version : 1.0
Nurbol 0:d33360d14a57 14 // Author : Jim Herd
Nurbol 0:d33360d14a57 15 // Date : 5th Oct 2011
Nurbol 0:d33360d14a57 16 //
Nurbol 0:d33360d14a57 17 #include "mbed.h"
Nurbol 0:d33360d14a57 18 #include "MCP23017.h"
Nurbol 0:d33360d14a57 19 #include "WattBob_TextLCD.h"
Nurbol 0:d33360d14a57 20 #include "cmd_io.h"
Nurbol 0:d33360d14a57 21 #include "globals.h"
Nurbol 0:d33360d14a57 22
Nurbol 0:d33360d14a57 23
Nurbol 0:d33360d14a57 24
Nurbol 0:d33360d14a57 25 Ticker timer1p;
Nurbol 0:d33360d14a57 26 Ticker timer2p;
Nurbol 0:d33360d14a57 27 Ticker timercounter1p;
Nurbol 0:d33360d14a57 28 Ticker timercounter2p;
Nurbol 0:d33360d14a57 29
Nurbol 0:d33360d14a57 30
Nurbol 0:d33360d14a57 31 AnalogIn sensor1(p15);
Nurbol 0:d33360d14a57 32 AnalogIn sensor2(p16);
Nurbol 0:d33360d14a57 33 AnalogOut valueLED1(p18);
Nurbol 0:d33360d14a57 34 DigitalOut valueLED2(p25);
Nurbol 0:d33360d14a57 35
Nurbol 0:d33360d14a57 36
Nurbol 0:d33360d14a57 37 DigitalOut led1(LED1);
Nurbol 0:d33360d14a57 38 DigitalOut led2(LED2);
Nurbol 0:d33360d14a57 39 DigitalOut led3(LED3);
Nurbol 0:d33360d14a57 40 DigitalOut led4(LED4);
Nurbol 0:d33360d14a57 41
Nurbol 0:d33360d14a57 42 DigitalOut clk(p24);
Nurbol 0:d33360d14a57 43
Nurbol 0:d33360d14a57 44 AnalogIn counter1p(p19);
Nurbol 0:d33360d14a57 45 AnalogIn counter2p(p20);
Nurbol 0:d33360d14a57 46
Nurbol 0:d33360d14a57 47
Nurbol 0:d33360d14a57 48 //******************************************************************************
Nurbol 0:d33360d14a57 49 // declare functions
Nurbol 0:d33360d14a57 50 //
Nurbol 0:d33360d14a57 51 void sensor1p (void);
Nurbol 0:d33360d14a57 52 void sensor2p (void);
Nurbol 0:d33360d14a57 53 void counter1 (void);
Nurbol 0:d33360d14a57 54 void counter2 (void);
Nurbol 0:d33360d14a57 55
Nurbol 0:d33360d14a57 56 //
Nurbol 0:d33360d14a57 57 //
Nurbol 0:d33360d14a57 58 // 2. five servo outputs
Nurbol 0:d33360d14a57 59 //
Nurbol 0:d33360d14a57 60
Nurbol 0:d33360d14a57 61 PwmOut servo_0(p26);
Nurbol 0:d33360d14a57 62 //PwmOut servo_1(p25);
Nurbol 0:d33360d14a57 63 //PwmOut servo_2(p24);
Nurbol 0:d33360d14a57 64 //PwmOut servo_3(p23);
Nurbol 0:d33360d14a57 65 PwmOut servo_4(p22);
Nurbol 0:d33360d14a57 66 //PwmOut servo_5(p21);
Nurbol 0:d33360d14a57 67
Nurbol 0:d33360d14a57 68 //
Nurbol 0:d33360d14a57 69 // 3. objects necessary to use the 2*16 character MBED display
Nurbol 0:d33360d14a57 70 //
Nurbol 0:d33360d14a57 71 MCP23017 *par_port;
Nurbol 0:d33360d14a57 72 WattBob_TextLCD *lcd;
Nurbol 0:d33360d14a57 73 //
Nurbol 0:d33360d14a57 74 // 4. Virtual COM port over USB link to laptop/PC
Nurbol 0:d33360d14a57 75 //
Nurbol 0:d33360d14a57 76 Serial pc(USBTX, USBRX);
Nurbol 0:d33360d14a57 77
Nurbol 0:d33360d14a57 78 //******************************************************************************
Nurbol 0:d33360d14a57 79 // Defined GLOBAL variables and structures
Nurbol 0:d33360d14a57 80 //
Nurbol 0:d33360d14a57 81 CMD_STRUCT ext_cmd; // structure to hold command data
Nurbol 0:d33360d14a57 82 STAT_STRUCT ext_stat; // structure to hold status reply
Nurbol 0:d33360d14a57 83
Nurbol 0:d33360d14a57 84 uint32_t last_servo; // store for last servo number
Nurbol 0:d33360d14a57 85
Nurbol 0:d33360d14a57 86 //************************************************************************
Nurbol 0:d33360d14a57 87 //************************************************************************
Nurbol 0:d33360d14a57 88 // init_sys : initialise the system
Nurbol 0:d33360d14a57 89 // ========
Nurbol 0:d33360d14a57 90 //
Nurbol 0:d33360d14a57 91 // 1. Configure 2*16 character display
Nurbol 0:d33360d14a57 92 // 2. Print "COM test" string
Nurbol 0:d33360d14a57 93 // 3. initialise relevant global variables
Nurbol 0:d33360d14a57 94 // 4. set COM port baud rate to 19200 bits per second
Nurbol 0:d33360d14a57 95 //
Nurbol 0:d33360d14a57 96 void init_sys(void) {
Nurbol 0:d33360d14a57 97
Nurbol 0:d33360d14a57 98 par_port = new MCP23017(p9, p10, 0x40);
Nurbol 0:d33360d14a57 99 lcd = new WattBob_TextLCD(par_port);
Nurbol 0:d33360d14a57 100
Nurbol 0:d33360d14a57 101 par_port->write_bit(1,BL_BIT); // turn LCD backlight ON
Nurbol 0:d33360d14a57 102 lcd->cls();
Nurbol 0:d33360d14a57 103 lcd->locate(0,0);
Nurbol 0:d33360d14a57 104 lcd->printf("COM ");
Nurbol 0:d33360d14a57 105
Nurbol 0:d33360d14a57 106 servo_0.period(0.020); // servo requires a 20ms period, common for all 5 servo objects
Nurbol 0:d33360d14a57 107 last_servo = SERVO_UNKNOWN;
Nurbol 0:d33360d14a57 108 pc.baud(19200);
Nurbol 0:d33360d14a57 109
Nurbol 0:d33360d14a57 110 return;
Nurbol 0:d33360d14a57 111 } // end init_sys
Nurbol 0:d33360d14a57 112
Nurbol 0:d33360d14a57 113 //************************************************************************
Nurbol 0:d33360d14a57 114 // process_cmd : decode and execute command
Nurbol 0:d33360d14a57 115 // ===========
Nurbol 0:d33360d14a57 116 uint32_t process_cmd(CMD_STRUCT *command)
Nurbol 0:d33360d14a57 117 {
Nurbol 0:d33360d14a57 118 int32_t pulse_width;
Nurbol 0:d33360d14a57 119
Nurbol 0:d33360d14a57 120 switch (command->cmd_code) {
Nurbol 0:d33360d14a57 121 //
Nurbol 0:d33360d14a57 122 // move a servo
Nurbol 0:d33360d14a57 123 //
Nurbol 0:d33360d14a57 124 case SERVO_CMD :
Nurbol 0:d33360d14a57 125 command->nos_data = 0; // no data to be returned
Nurbol 0:d33360d14a57 126 //
Nurbol 0:d33360d14a57 127 // check that parameters are OK
Nurbol 0:d33360d14a57 128 //
Nurbol 0:d33360d14a57 129 if (command->nos_params != 2) { // check for 2 parameters
Nurbol 0:d33360d14a57 130 command->result_status = CMD_BAD_NUMBER_OF_PARAMETERS;
Nurbol 0:d33360d14a57 131 break;
Nurbol 0:d33360d14a57 132 }
Nurbol 0:d33360d14a57 133 if (command->param[0] > MAX_SERVO_NUMBER ) { // check servo number
Nurbol 0:d33360d14a57 134 command->result_status = CMD_BAD_SERVO_NUMBER;
Nurbol 0:d33360d14a57 135 break;
Nurbol 0:d33360d14a57 136 }
Nurbol 0:d33360d14a57 137 if ((command->param[1] < MIN_SERVO_ANGLE) ||
Nurbol 0:d33360d14a57 138 (command->param[1] > MAX_SERVO_ANGLE) ) {
Nurbol 0:d33360d14a57 139 command->result_status = CMD_BAD_SERVO_VALUE;
Nurbol 0:d33360d14a57 140 break;
Nurbol 0:d33360d14a57 141 }
Nurbol 0:d33360d14a57 142 if ((command->param[0] == 4) && (command->param[1] == 0)) {
Nurbol 0:d33360d14a57 143 pulse_width = 0; // convert angle to pulse width
Nurbol 0:d33360d14a57 144 }
Nurbol 0:d33360d14a57 145 else{
Nurbol 0:d33360d14a57 146 pulse_width = 1000 + (command->param[1] * 1000) / MAX_SERVO_ANGLE; // convert angle to pulse width
Nurbol 0:d33360d14a57 147 }
Nurbol 0:d33360d14a57 148
Nurbol 0:d33360d14a57 149
Nurbol 0:d33360d14a57 150
Nurbol 0:d33360d14a57 151 //
Nurbol 0:d33360d14a57 152 // implement servo move to all 5 servos
Nurbol 0:d33360d14a57 153 //
Nurbol 0:d33360d14a57 154 switch (command->param[0]) {
Nurbol 0:d33360d14a57 155 case 0 : servo_0.pulsewidth_us(pulse_width); break;
Nurbol 0:d33360d14a57 156 // case 1 : servo_1.pulsewidth_us(pulse_width); break;
Nurbol 0:d33360d14a57 157 // case 2 : servo_2.pulsewidth_us(pulse_width); break;
Nurbol 0:d33360d14a57 158 // case 3 : servo_3.pulsewidth_us(pulse_width); break;
Nurbol 0:d33360d14a57 159 case 4 : servo_4.pulsewidth_us(pulse_width); break;
Nurbol 0:d33360d14a57 160 // case 5 : servo_5.pulsewidth_us(pulse_width); break;
Nurbol 0:d33360d14a57 161
Nurbol 0:d33360d14a57 162 }
Nurbol 0:d33360d14a57 163 last_servo = command->param[0];
Nurbol 0:d33360d14a57 164 break;
Nurbol 0:d33360d14a57 165 //
Nurbol 0:d33360d14a57 166 // return last servo number
Nurbol 0:d33360d14a57 167 //
Nurbol 0:d33360d14a57 168 case READ_CMD :
Nurbol 0:d33360d14a57 169 command->nos_data = 1; // no data to be returned
Nurbol 0:d33360d14a57 170 command->result_data[0] = valueLED1;
Nurbol 0:d33360d14a57 171 command->result_data[1] = valueLED2;
Nurbol 0:d33360d14a57 172 break;
Nurbol 0:d33360d14a57 173 //
Nurbol 0:d33360d14a57 174 // catch any problems
Nurbol 0:d33360d14a57 175 //
Nurbol 0:d33360d14a57 176 default:
Nurbol 0:d33360d14a57 177 command->nos_data = 0; // no data to be returned
Nurbol 0:d33360d14a57 178 command->result_status = CMD_BAD_SERVO_VALUE;
Nurbol 0:d33360d14a57 179 break;
Nurbol 0:d33360d14a57 180 }
Nurbol 0:d33360d14a57 181 return OK;
Nurbol 0:d33360d14a57 182 }
Nurbol 0:d33360d14a57 183
Nurbol 0:d33360d14a57 184 //************************************************************************
Nurbol 0:d33360d14a57 185
Nurbol 0:d33360d14a57 186 //function
Nurbol 0:d33360d14a57 187 void sensor1p (void){
Nurbol 0:d33360d14a57 188
Nurbol 0:d33360d14a57 189
Nurbol 0:d33360d14a57 190 clk = !clk;
Nurbol 0:d33360d14a57 191 wait(0.01);
Nurbol 0:d33360d14a57 192 sensor1.read();
Nurbol 0:d33360d14a57 193 if(sensor1 > 0.5) {
Nurbol 0:d33360d14a57 194 led1 = 1;
Nurbol 0:d33360d14a57 195 valueLED1 = 1;
Nurbol 0:d33360d14a57 196 }
Nurbol 0:d33360d14a57 197 else if(sensor1 < 0.5){
Nurbol 0:d33360d14a57 198 led1 = 0;
Nurbol 0:d33360d14a57 199 valueLED1 = 0;
Nurbol 0:d33360d14a57 200 }
Nurbol 0:d33360d14a57 201 }
Nurbol 0:d33360d14a57 202
Nurbol 0:d33360d14a57 203
Nurbol 0:d33360d14a57 204 void sensor2p (){
Nurbol 0:d33360d14a57 205
Nurbol 0:d33360d14a57 206 sensor2.read();
Nurbol 0:d33360d14a57 207 if(sensor2 > 0.5) {
Nurbol 0:d33360d14a57 208 led2 = 1;
Nurbol 0:d33360d14a57 209 valueLED2 = 1;
Nurbol 0:d33360d14a57 210 }
Nurbol 0:d33360d14a57 211 else if(sensor2 < 0.5){
Nurbol 0:d33360d14a57 212 led2 = 0;
Nurbol 0:d33360d14a57 213 valueLED2 = 0;
Nurbol 0:d33360d14a57 214 }
Nurbol 0:d33360d14a57 215 }
Nurbol 0:d33360d14a57 216
Nurbol 0:d33360d14a57 217 void counter1 (){
Nurbol 0:d33360d14a57 218 if(counter1p > 0.5){
Nurbol 0:d33360d14a57 219 led3 = 1;
Nurbol 0:d33360d14a57 220 }
Nurbol 0:d33360d14a57 221 else{
Nurbol 0:d33360d14a57 222 led3 = 0;
Nurbol 0:d33360d14a57 223 }
Nurbol 0:d33360d14a57 224 }
Nurbol 0:d33360d14a57 225
Nurbol 0:d33360d14a57 226 void counter2(){
Nurbol 0:d33360d14a57 227 if(counter2p > 0.5){
Nurbol 0:d33360d14a57 228 led4 = 1;
Nurbol 0:d33360d14a57 229 process_cmd(&ext_cmd);
Nurbol 0:d33360d14a57 230 }
Nurbol 0:d33360d14a57 231 else{
Nurbol 0:d33360d14a57 232 led4 = 0;
Nurbol 0:d33360d14a57 233 }
Nurbol 0:d33360d14a57 234 }
Nurbol 0:d33360d14a57 235
Nurbol 0:d33360d14a57 236
Nurbol 0:d33360d14a57 237
Nurbol 0:d33360d14a57 238
Nurbol 0:d33360d14a57 239 //************************************************************************
Nurbol 0:d33360d14a57 240 //
Nurbol 0:d33360d14a57 241 int main() {
Nurbol 0:d33360d14a57 242
Nurbol 0:d33360d14a57 243 valueLED1=0;
Nurbol 0:d33360d14a57 244 valueLED2=0;
Nurbol 0:d33360d14a57 245 clk=0;
Nurbol 0:d33360d14a57 246 init_sys();
Nurbol 0:d33360d14a57 247
Nurbol 0:d33360d14a57 248
Nurbol 0:d33360d14a57 249
Nurbol 0:d33360d14a57 250
Nurbol 0:d33360d14a57 251
Nurbol 0:d33360d14a57 252 FOREVER {
Nurbol 0:d33360d14a57 253
Nurbol 0:d33360d14a57 254 timer2p.attach(&sensor2p, 0.1);
Nurbol 0:d33360d14a57 255 timer1p.attach(&sensor1p, 0.1);
Nurbol 0:d33360d14a57 256
Nurbol 0:d33360d14a57 257
Nurbol 0:d33360d14a57 258 clk = !clk;
Nurbol 0:d33360d14a57 259 wait(0.1);
Nurbol 0:d33360d14a57 260
Nurbol 0:d33360d14a57 261 timercounter2p.attach(&counter2, 0.1);
Nurbol 0:d33360d14a57 262 timercounter1p.attach(&counter1, 0.1);
Nurbol 0:d33360d14a57 263
Nurbol 0:d33360d14a57 264
Nurbol 0:d33360d14a57 265
Nurbol 0:d33360d14a57 266
Nurbol 0:d33360d14a57 267
Nurbol 0:d33360d14a57 268
Nurbol 0:d33360d14a57 269
Nurbol 0:d33360d14a57 270 get_cmd(&ext_cmd);
Nurbol 0:d33360d14a57 271 //
Nurbol 0:d33360d14a57 272 // Check status of read command activity and return an error status if there was a problem
Nurbol 0:d33360d14a57 273 // If there is a problem, then return status code only and wait for next command.
Nurbol 0:d33360d14a57 274 //
Nurbol 0:d33360d14a57 275 if (ext_cmd.result_status != OK){
Nurbol 0:d33360d14a57 276 send_status(ext_cmd.result_status);
Nurbol 0:d33360d14a57 277 continue;
Nurbol 0:d33360d14a57 278 }
Nurbol 0:d33360d14a57 279 //
Nurbol 0:d33360d14a57 280 // Parse command and return an error staus if there is a problem
Nurbol 0:d33360d14a57 281 // If there is a problem, then return status code only and wait for next command.
Nurbol 0:d33360d14a57 282 //
Nurbol 0:d33360d14a57 283 parse_cmd(&ext_cmd);
Nurbol 0:d33360d14a57 284 lcd->locate(1,0); lcd->printf(ext_cmd.cmd_str);
Nurbol 0:d33360d14a57 285 if ((ext_cmd.result_status != OK) && (ext_cmd.cmd_code != TEXT_CMD)){
Nurbol 0:d33360d14a57 286 lcd->locate(1,0); lcd->printf("parse : error");
Nurbol 0:d33360d14a57 287 send_status(ext_cmd.result_status);
Nurbol 0:d33360d14a57 288 continue;
Nurbol 0:d33360d14a57 289 }
Nurbol 0:d33360d14a57 290 //
Nurbol 0:d33360d14a57 291 // Execute command and return an error staus if there is a problem
Nurbol 0:d33360d14a57 292 //
Nurbol 0:d33360d14a57 293 process_cmd(&ext_cmd);
Nurbol 0:d33360d14a57 294 reply_to_cmd(&ext_cmd);
Nurbol 0:d33360d14a57 295
Nurbol 0:d33360d14a57 296
Nurbol 0:d33360d14a57 297
Nurbol 0:d33360d14a57 298 }
Nurbol 0:d33360d14a57 299
Nurbol 0:d33360d14a57 300 }
Nurbol 0:d33360d14a57 301
Nurbol 0:d33360d14a57 302
Nurbol 0:d33360d14a57 303
Nurbol 0:d33360d14a57 304
Nurbol 0:d33360d14a57 305
Nurbol 0:d33360d14a57 306
Nurbol 0:d33360d14a57 307
Nurbol 0:d33360d14a57 308
Nurbol 0:d33360d14a57 309