Servo_Control.h

00001 #pragma once
00002 #ifndef SERVO_CONTROL_H
00003 #define SERVO_CONTROL_H
00004 //#include <processes.h>
00005 #include "Servo.h"
00006 
00007 #define SERVO_PIN p21
00008 extern BusOut leds;
00009 template<> OS_PROCESS void Servo_control::Exec() //serial stream handling process
00010 {
00011    Servo Servo_object(SERVO_PIN);
00012    char Servo_Command[100] = "";
00013    Servo_Command[99] = '\0';     
00014    float servo_position = 0, range = 0, degrees = 0;
00015    char *pEnd;
00016    //char output_chars[50] = "";
00017    
00018    for (;;)
00019    {
00020         
00021        //Servo_Command_Event.Wait();
00022        for(byte i = 0; i<100; i++)
00023        {
00024            SER_channel.pop(Servo_Command[i]);
00025             leds = 0x6;
00026             if(Servo_Command[i] == '\0')  //keep popping chars until a delimiter is found
00027                 break;
00028             else if(Servo_Command[i] != '\0' && i == 99)
00029                 Servo_Command[i] = '\0';
00030                 
00031        }
00032              
00033         //sprintf(output_chars, "a servo command was received:\n");
00034         //TX_Channel.write(output_chars, strlen(output_chars)+1); //output the command for debugging
00035         //TX_Channel.write(Servo_Command, strlen(Servo_Command)+1); //output the command for debugging
00036         
00037         if(strlen(Servo_Command) > 6)
00038         {
00039         
00040             if (strlen(Servo_Command) >= 20 && strstr(Servo_Command,":Calibrate")) 
00041             {    //Serv:Calibrate:range:degrees
00042                    
00043                     range = strtod(&Servo_Command[15], &pEnd);
00044                     if (range >= 0 && range <= 1 && pEnd - &Servo_Command[15] != 0)
00045                     {
00046                         const char* deg = strstr(strstr(strstr(Servo_Command,":"),":"),":");
00047                         degrees = strtod(deg+1, &pEnd);
00048                         if (degrees >= -360 && range <= 360 && (pEnd - deg) != 0)
00049                         {
00050                             //printf("Received command: Serv:Calibrate:%f:%f. \n", range, degrees);
00051                             Servo_object.calibrate(range,degrees);
00052                         }
00053                     }
00054                 
00055             }
00056             
00057             else
00058             {
00059                 servo_position = strtod(&Servo_Command[4], &pEnd); //&Servo_Command[5]
00060                 
00061                 //sprintf(output_chars, "servo position is %f\n", servo_position);         
00062                 //TX_Channel.write(output_chars, strlen(output_chars)+1); //output the command for debugging
00063 
00064                 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
00065                 {
00066                     Servo_object = servo_position;
00067                 }
00068                 else if (servo_position == -1) //sweep the position through range
00069                 {
00070                     for(float p=0; p<1.0; p += 0.1)
00071                        {
00072                             Servo_object = p;
00073                             Sleep(200);
00074                        }
00075                        
00076                 }
00077              }
00078         }
00079         leds = 0x6;
00080    }
00081    
00082    
00083     
00084 }  
00085 
00086 
00087 
00088 #endif
00089