Nurbol Nurdaulet / Mbed 2 deprecated 2sensors_2servo_21_11_11
Committer:
Nurbol
Date:
Mon Nov 21 19:25:35 2011 +0000
Revision:
2:a6531d45321f
Parent:
1:fc8017eb0c2a

        

Who changed what in which revision?

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