Program to control an accelerometer, motors and a rangefinder using the ScmRTOS ported to mbed. (Work in progress and buggy)
Diff: Hexacopter/Servo_Control.h
- Revision:
- 0:9b057566f9ee
diff -r 000000000000 -r 9b057566f9ee Hexacopter/Servo_Control.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Hexacopter/Servo_Control.h Mon Nov 01 20:39:01 2010 +0000 @@ -0,0 +1,89 @@ +#pragma once +#ifndef SERVO_CONTROL_H +#define SERVO_CONTROL_H +//#include <processes.h> +#include "Servo.h" + +#define SERVO_PIN p21 +extern BusOut leds; +template<> OS_PROCESS void Servo_control::Exec() //serial stream handling process +{ + Servo Servo_object(SERVO_PIN); + char Servo_Command[100] = ""; + Servo_Command[99] = '\0'; + float servo_position = 0, range = 0, degrees = 0; + char *pEnd; + //char output_chars[50] = ""; + + for (;;) + { + + //Servo_Command_Event.Wait(); + for(byte i = 0; i<100; i++) + { + SER_channel.pop(Servo_Command[i]); + leds = 0x6; + if(Servo_Command[i] == '\0') //keep popping chars until a delimiter is found + break; + else if(Servo_Command[i] != '\0' && i == 99) + Servo_Command[i] = '\0'; + + } + + //sprintf(output_chars, "a servo command was received:\n"); + //TX_Channel.write(output_chars, strlen(output_chars)+1); //output the command for debugging + //TX_Channel.write(Servo_Command, strlen(Servo_Command)+1); //output the command for debugging + + if(strlen(Servo_Command) > 6) + { + + if (strlen(Servo_Command) >= 20 && strstr(Servo_Command,":Calibrate")) + { //Serv:Calibrate:range:degrees + + range = strtod(&Servo_Command[15], &pEnd); + if (range >= 0 && range <= 1 && pEnd - &Servo_Command[15] != 0) + { + const char* deg = strstr(strstr(strstr(Servo_Command,":"),":"),":"); + degrees = strtod(deg+1, &pEnd); + if (degrees >= -360 && range <= 360 && (pEnd - deg) != 0) + { + //printf("Received command: Serv:Calibrate:%f:%f. \n", range, degrees); + Servo_object.calibrate(range,degrees); + } + } + + } + + else + { + servo_position = strtod(&Servo_Command[4], &pEnd); //&Servo_Command[5] + + //sprintf(output_chars, "servo position is %f\n", servo_position); + //TX_Channel.write(output_chars, strlen(output_chars)+1); //output the command for debugging + + 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 + { + Servo_object = servo_position; + } + else if (servo_position == -1) //sweep the position through range + { + for(float p=0; p<1.0; p += 0.1) + { + Servo_object = p; + Sleep(200); + } + + } + } + } + leds = 0x6; + } + + + +} + + + +#endif +