Greg Kuhn
Program to control an accelerometer, motors and a rangefinder using the ScmRTOS ported to mbed. (Work in progress and buggy)
Hexacopter/Servo_Control.h@0:9b057566f9ee, 2010-11-01 (annotated)
- Committer:
- jberry
- Date:
- Mon Nov 01 20:39:01 2010 +0000
- Revision:
- 0:9b057566f9ee
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| jberry | 0:9b057566f9ee | 1 | #pragma once |
| jberry | 0:9b057566f9ee | 2 | #ifndef SERVO_CONTROL_H |
| jberry | 0:9b057566f9ee | 3 | #define SERVO_CONTROL_H |
| jberry | 0:9b057566f9ee | 4 | //#include <processes.h> |
| jberry | 0:9b057566f9ee | 5 | #include "Servo.h" |
| jberry | 0:9b057566f9ee | 6 | |
| jberry | 0:9b057566f9ee | 7 | #define SERVO_PIN p21 |
| jberry | 0:9b057566f9ee | 8 | extern BusOut leds; |
| jberry | 0:9b057566f9ee | 9 | template<> OS_PROCESS void Servo_control::Exec() //serial stream handling process |
| jberry | 0:9b057566f9ee | 10 | { |
| jberry | 0:9b057566f9ee | 11 | Servo Servo_object(SERVO_PIN); |
| jberry | 0:9b057566f9ee | 12 | char Servo_Command[100] = ""; |
| jberry | 0:9b057566f9ee | 13 | Servo_Command[99] = '\0'; |
| jberry | 0:9b057566f9ee | 14 | float servo_position = 0, range = 0, degrees = 0; |
| jberry | 0:9b057566f9ee | 15 | char *pEnd; |
| jberry | 0:9b057566f9ee | 16 | //char output_chars[50] = ""; |
| jberry | 0:9b057566f9ee | 17 | |
| jberry | 0:9b057566f9ee | 18 | for (;;) |
| jberry | 0:9b057566f9ee | 19 | { |
| jberry | 0:9b057566f9ee | 20 | |
| jberry | 0:9b057566f9ee | 21 | //Servo_Command_Event.Wait(); |
| jberry | 0:9b057566f9ee | 22 | for(byte i = 0; i<100; i++) |
| jberry | 0:9b057566f9ee | 23 | { |
| jberry | 0:9b057566f9ee | 24 | SER_channel.pop(Servo_Command[i]); |
| jberry | 0:9b057566f9ee | 25 | leds = 0x6; |
| jberry | 0:9b057566f9ee | 26 | if(Servo_Command[i] == '\0') //keep popping chars until a delimiter is found |
| jberry | 0:9b057566f9ee | 27 | break; |
| jberry | 0:9b057566f9ee | 28 | else if(Servo_Command[i] != '\0' && i == 99) |
| jberry | 0:9b057566f9ee | 29 | Servo_Command[i] = '\0'; |
| jberry | 0:9b057566f9ee | 30 | |
| jberry | 0:9b057566f9ee | 31 | } |
| jberry | 0:9b057566f9ee | 32 | |
| jberry | 0:9b057566f9ee | 33 | //sprintf(output_chars, "a servo command was received:\n"); |
| jberry | 0:9b057566f9ee | 34 | //TX_Channel.write(output_chars, strlen(output_chars)+1); //output the command for debugging |
| jberry | 0:9b057566f9ee | 35 | //TX_Channel.write(Servo_Command, strlen(Servo_Command)+1); //output the command for debugging |
| jberry | 0:9b057566f9ee | 36 | |
| jberry | 0:9b057566f9ee | 37 | if(strlen(Servo_Command) > 6) |
| jberry | 0:9b057566f9ee | 38 | { |
| jberry | 0:9b057566f9ee | 39 | |
| jberry | 0:9b057566f9ee | 40 | if (strlen(Servo_Command) >= 20 && strstr(Servo_Command,":Calibrate")) |
| jberry | 0:9b057566f9ee | 41 | { //Serv:Calibrate:range:degrees |
| jberry | 0:9b057566f9ee | 42 | |
| jberry | 0:9b057566f9ee | 43 | range = strtod(&Servo_Command[15], &pEnd); |
| jberry | 0:9b057566f9ee | 44 | if (range >= 0 && range <= 1 && pEnd - &Servo_Command[15] != 0) |
| jberry | 0:9b057566f9ee | 45 | { |
| jberry | 0:9b057566f9ee | 46 | const char* deg = strstr(strstr(strstr(Servo_Command,":"),":"),":"); |
| jberry | 0:9b057566f9ee | 47 | degrees = strtod(deg+1, &pEnd); |
| jberry | 0:9b057566f9ee | 48 | if (degrees >= -360 && range <= 360 && (pEnd - deg) != 0) |
| jberry | 0:9b057566f9ee | 49 | { |
| jberry | 0:9b057566f9ee | 50 | //printf("Received command: Serv:Calibrate:%f:%f. \n", range, degrees); |
| jberry | 0:9b057566f9ee | 51 | Servo_object.calibrate(range,degrees); |
| jberry | 0:9b057566f9ee | 52 | } |
| jberry | 0:9b057566f9ee | 53 | } |
| jberry | 0:9b057566f9ee | 54 | |
| jberry | 0:9b057566f9ee | 55 | } |
| jberry | 0:9b057566f9ee | 56 | |
| jberry | 0:9b057566f9ee | 57 | else |
| jberry | 0:9b057566f9ee | 58 | { |
| jberry | 0:9b057566f9ee | 59 | servo_position = strtod(&Servo_Command[4], &pEnd); //&Servo_Command[5] |
| jberry | 0:9b057566f9ee | 60 | |
| jberry | 0:9b057566f9ee | 61 | //sprintf(output_chars, "servo position is %f\n", servo_position); |
| jberry | 0:9b057566f9ee | 62 | //TX_Channel.write(output_chars, strlen(output_chars)+1); //output the command for debugging |
| jberry | 0:9b057566f9ee | 63 | |
| jberry | 0:9b057566f9ee | 64 | if (servo_position >= 0 && servo_position <= 1 && (pEnd - &Servo_Command[4]) != 0) //the magic number 4 relates to the position in the string where the actual data should start |
| jberry | 0:9b057566f9ee | 65 | { |
| jberry | 0:9b057566f9ee | 66 | Servo_object = servo_position; |
| jberry | 0:9b057566f9ee | 67 | } |
| jberry | 0:9b057566f9ee | 68 | else if (servo_position == -1) //sweep the position through range |
| jberry | 0:9b057566f9ee | 69 | { |
| jberry | 0:9b057566f9ee | 70 | for(float p=0; p<1.0; p += 0.1) |
| jberry | 0:9b057566f9ee | 71 | { |
| jberry | 0:9b057566f9ee | 72 | Servo_object = p; |
| jberry | 0:9b057566f9ee | 73 | Sleep(200); |
| jberry | 0:9b057566f9ee | 74 | } |
| jberry | 0:9b057566f9ee | 75 | |
| jberry | 0:9b057566f9ee | 76 | } |
| jberry | 0:9b057566f9ee | 77 | } |
| jberry | 0:9b057566f9ee | 78 | } |
| jberry | 0:9b057566f9ee | 79 | leds = 0x6; |
| jberry | 0:9b057566f9ee | 80 | } |
| jberry | 0:9b057566f9ee | 81 | |
| jberry | 0:9b057566f9ee | 82 | |
| jberry | 0:9b057566f9ee | 83 | |
| jberry | 0:9b057566f9ee | 84 | } |
| jberry | 0:9b057566f9ee | 85 | |
| jberry | 0:9b057566f9ee | 86 | |
| jberry | 0:9b057566f9ee | 87 | |
| jberry | 0:9b057566f9ee | 88 | #endif |
| jberry | 0:9b057566f9ee | 89 |