main.cpp

00001 /* just a test for "mbed R/C Servo Library"
00002  * send from PC to mbed a string like ":XXYY"<RC>
00003  * where XX is the hexa value [00..FF ] (0..255) of Servo1
00004  * and YY is the hexa value [00..FF ] (0..255) of Servo2
00005  * also "V"<RC> gives the version.
00006  */
00007 
00008 #include "mbed.h"
00009 #include "Servo.h"
00010   
00011 PwmOut led_1(LED1);
00012 PwmOut led_2(LED2);
00013 
00014 Servo servo_1(p21);
00015 Servo servo_2(p22);
00016 
00017 Serial rs_pc(USBTX, USBRX); // virtual com (tx, rx)
00018 
00019 // convert 2 ascii chars hex format ("00".."FF") -> 1 unsigned char (0..255)
00020 unsigned char hex_bin (unsigned char  *ptr )
00021 {
00022     unsigned char temp;
00023 
00024     if (*ptr>='0' && *ptr<='9') temp = *ptr & 0x0F;
00025                           else  temp = (*ptr & 0x0F) + 9;
00026     temp <<= 4;
00027     if (*(ptr+1)>='0' && *(ptr+1)<='9') temp |= *(ptr+1) & 0x0F;
00028                                   else  temp |= (*(ptr+1) & 0x0F) + 9;
00029     return temp;
00030 }
00031 
00032 int main() {
00033     unsigned char i=0;
00034     float p;
00035     unsigned char c;
00036     unsigned char cmd[6];
00037     while(1) {
00038         if(rs_pc.readable()) { // char on rs_pc
00039             c = rs_pc.getc();
00040             if (c==13){ // end of tram
00041                 if (cmd[0]== ':'){ // cmd servos
00042                     rs_pc.putc(13); // ok
00043                     // servo 1
00044                     p = hex_bin(&cmd[1])/255.0; // 0.00 (min) -> 1.00 (max)
00045                     servo_1 = p;
00046                     led_1   = p; // just for eyes
00047                     // servo 2
00048                     p = hex_bin(&cmd[3])/255.0; // 0.00 (min) -> 1.00 (max)
00049                     servo_2 = p;
00050                     led_2   = p;
00051                 }
00052                 if (cmd[0]== 'V'){ // version
00053                     rs_pc.printf("Servos test V1.01.a\r");
00054                 }
00055                 i=0;
00056             }else{
00057                 if (c==':') i=0; // start of tram
00058                 cmd[i++]=c;
00059                 if (i>5){
00060                     rs_pc.printf("Wrong command\r");
00061                     i=0;
00062                 }
00063             }
00064         }
00065     }
00066 }