Diff: Hexacopter/Servo_Control.h

Revision:

0:9b057566f9ee

--- /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
+