Rousseaux Jean-Claude
/
TestServo
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
main.cpp@0:39cd1e782d1e, 2010-10-18 (annotated)
- Committer:
- jcrx
- Date:
- Mon Oct 18 13:17:22 2010 +0000
- Revision:
- 0:39cd1e782d1e
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| jcrx | 0:39cd1e782d1e | 1 | /* just a test for "mbed R/C Servo Library" |
| jcrx | 0:39cd1e782d1e | 2 | * send from PC to mbed a string like ":XXYY"<RC> |
| jcrx | 0:39cd1e782d1e | 3 | * where XX is the hexa value [00..FF ] (0..255) of Servo1 |
| jcrx | 0:39cd1e782d1e | 4 | * and YY is the hexa value [00..FF ] (0..255) of Servo2 |
| jcrx | 0:39cd1e782d1e | 5 | * also "V"<RC> gives the version. |
| jcrx | 0:39cd1e782d1e | 6 | */ |
| jcrx | 0:39cd1e782d1e | 7 | |
| jcrx | 0:39cd1e782d1e | 8 | #include "mbed.h" |
| jcrx | 0:39cd1e782d1e | 9 | #include "Servo.h" |
| jcrx | 0:39cd1e782d1e | 10 | |
| jcrx | 0:39cd1e782d1e | 11 | PwmOut led_1(LED1); |
| jcrx | 0:39cd1e782d1e | 12 | PwmOut led_2(LED2); |
| jcrx | 0:39cd1e782d1e | 13 | |
| jcrx | 0:39cd1e782d1e | 14 | Servo servo_1(p21); |
| jcrx | 0:39cd1e782d1e | 15 | Servo servo_2(p22); |
| jcrx | 0:39cd1e782d1e | 16 | |
| jcrx | 0:39cd1e782d1e | 17 | Serial rs_pc(USBTX, USBRX); // virtual com (tx, rx) |
| jcrx | 0:39cd1e782d1e | 18 | |
| jcrx | 0:39cd1e782d1e | 19 | // convert 2 ascii chars hex format ("00".."FF") -> 1 unsigned char (0..255) |
| jcrx | 0:39cd1e782d1e | 20 | unsigned char hex_bin (unsigned char *ptr ) |
| jcrx | 0:39cd1e782d1e | 21 | { |
| jcrx | 0:39cd1e782d1e | 22 | unsigned char temp; |
| jcrx | 0:39cd1e782d1e | 23 | |
| jcrx | 0:39cd1e782d1e | 24 | if (*ptr>='0' && *ptr<='9') temp = *ptr & 0x0F; |
| jcrx | 0:39cd1e782d1e | 25 | else temp = (*ptr & 0x0F) + 9; |
| jcrx | 0:39cd1e782d1e | 26 | temp <<= 4; |
| jcrx | 0:39cd1e782d1e | 27 | if (*(ptr+1)>='0' && *(ptr+1)<='9') temp |= *(ptr+1) & 0x0F; |
| jcrx | 0:39cd1e782d1e | 28 | else temp |= (*(ptr+1) & 0x0F) + 9; |
| jcrx | 0:39cd1e782d1e | 29 | return temp; |
| jcrx | 0:39cd1e782d1e | 30 | } |
| jcrx | 0:39cd1e782d1e | 31 | |
| jcrx | 0:39cd1e782d1e | 32 | int main() { |
| jcrx | 0:39cd1e782d1e | 33 | unsigned char i=0; |
| jcrx | 0:39cd1e782d1e | 34 | float p; |
| jcrx | 0:39cd1e782d1e | 35 | unsigned char c; |
| jcrx | 0:39cd1e782d1e | 36 | unsigned char cmd[6]; |
| jcrx | 0:39cd1e782d1e | 37 | while(1) { |
| jcrx | 0:39cd1e782d1e | 38 | if(rs_pc.readable()) { // char on rs_pc |
| jcrx | 0:39cd1e782d1e | 39 | c = rs_pc.getc(); |
| jcrx | 0:39cd1e782d1e | 40 | if (c==13){ // end of tram |
| jcrx | 0:39cd1e782d1e | 41 | if (cmd[0]== ':'){ // cmd servos |
| jcrx | 0:39cd1e782d1e | 42 | rs_pc.putc(13); // ok |
| jcrx | 0:39cd1e782d1e | 43 | // servo 1 |
| jcrx | 0:39cd1e782d1e | 44 | p = hex_bin(&cmd[1])/255.0; // 0.00 (min) -> 1.00 (max) |
| jcrx | 0:39cd1e782d1e | 45 | servo_1 = p; |
| jcrx | 0:39cd1e782d1e | 46 | led_1 = p; // just for eyes |
| jcrx | 0:39cd1e782d1e | 47 | // servo 2 |
| jcrx | 0:39cd1e782d1e | 48 | p = hex_bin(&cmd[3])/255.0; // 0.00 (min) -> 1.00 (max) |
| jcrx | 0:39cd1e782d1e | 49 | servo_2 = p; |
| jcrx | 0:39cd1e782d1e | 50 | led_2 = p; |
| jcrx | 0:39cd1e782d1e | 51 | } |
| jcrx | 0:39cd1e782d1e | 52 | if (cmd[0]== 'V'){ // version |
| jcrx | 0:39cd1e782d1e | 53 | rs_pc.printf("Servos test V1.01.a\r"); |
| jcrx | 0:39cd1e782d1e | 54 | } |
| jcrx | 0:39cd1e782d1e | 55 | i=0; |
| jcrx | 0:39cd1e782d1e | 56 | }else{ |
| jcrx | 0:39cd1e782d1e | 57 | if (c==':') i=0; // start of tram |
| jcrx | 0:39cd1e782d1e | 58 | cmd[i++]=c; |
| jcrx | 0:39cd1e782d1e | 59 | if (i>5){ |
| jcrx | 0:39cd1e782d1e | 60 | rs_pc.printf("Wrong command\r"); |
| jcrx | 0:39cd1e782d1e | 61 | i=0; |
| jcrx | 0:39cd1e782d1e | 62 | } |
| jcrx | 0:39cd1e782d1e | 63 | } |
| jcrx | 0:39cd1e782d1e | 64 | } |
| jcrx | 0:39cd1e782d1e | 65 | } |
| jcrx | 0:39cd1e782d1e | 66 | } |