Diff: Hexacopter/Motor_control.h

Revision:

0:9b057566f9ee

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Hexacopter/Motor_control.h	Mon Nov 01 20:39:01 2010 +0000
@@ -0,0 +1,224 @@
+#pragma once
+#ifndef MOTOR_CONTROL_H
+#define MOTOR_CONTROL_H
+#include <cmath>
+#include <processes.h>
+
+
+//Motor_controller M_cont_process;
+enum direction{CLOCKWISE,ANTICLOCKWISE};
+ //PwmOut Out1(p23); //motor1
+ //PwmOut Out2(p24);
+ //PwmOut Out3(p25);
+ //PwmOut Out4(p26);
+
+class motor_control
+{
+    public:
+    motor_control(PinName OUT1, PinName OUT2)
+    :Out1(OUT1),Out2(OUT2),Output(&Out1)
+    {
+        //Output = &Out1;
+        set_direction(CLOCKWISE);
+        SPEED = 0.0;
+        Out1.pulsewidth_ms(1); // pulsewidth should be 1ms
+        Out1.write(0.0); //set initial duty cycle to 0 (i.e. off)
+        Out2.write(0.0); //set initial duty cycle to 0 (i.e. off)
+        
+        
+
+        
+     }
+        
+    void set_direction(direction D)
+    {
+        if( D == CLOCKWISE )
+        {
+            *Output = 0;
+            Output = &Out2;            
+         }
+        else
+        {
+            *Output = 0;
+            Output = &Out1;            
+        }
+        DIR = D;
+        //set_speed(get_speed());
+     }
+    
+     direction get_direction()
+     {
+            return DIR;
+     }
+     
+     void set_speed(float speed)
+     {
+        SPEED = speed;
+        if(speed >= -1 && speed < 0)
+        {
+           if(get_direction() == CLOCKWISE) //ALLOW SOME TIME for the motor to change direction
+           {
+                *Output = 0;
+                //wait_ms(500);
+           }
+           
+           set_direction(ANTICLOCKWISE);
+           *Output = speed;
+        }
+        else if(speed >= 0 && speed <= 1)
+        {
+            if(get_direction() == CLOCKWISE) //ALLOW SOME TIME for the motor to change direction
+            {
+                *Output = 0;
+                //wait_ms(500);
+            }
+            
+            set_direction(CLOCKWISE);
+            *Output = speed;
+        }
+        
+      }
+      float get_speed()
+      {
+        return SPEED;
+      }
+      
+      void speed_inc()
+      {
+        if( (get_speed() + 0.1) <= 1)
+        set_speed(SPEED + 0.1);
+        else if( get_speed() + 0.1 > 1)
+        set_speed(1);        
+      }
+      
+      void speed_dec()
+      {
+        if( (get_speed() - 0.1) >= 0)
+        set_speed(SPEED - 0.1);
+        else if( get_speed() - 0.1 < 0)
+        set_speed(0);        
+        
+      }
+      
+      void change_direction()
+      {
+        if (get_direction() == CLOCKWISE)
+            set_direction(ANTICLOCKWISE);
+        else
+            set_direction(CLOCKWISE);
+      }
+            
+    
+    private:
+    //int PIN1,PIN2;
+    direction DIR;
+    PwmOut Out1;
+    PwmOut Out2;
+    PwmOut* Output;
+    
+
+    float SPEED;
+    
+ };
+
+
+char MOT_Return_chars[100]; //defined in accelerometer.h
+char Motor_command[100] = "";
+byte Motor_number = 0;
+
+extern BusOut leds;
+template<> OS_PROCESS void Motor_controller::Exec() //motor control handling process
+{
+    Motor_command[99] = '\0';
+   motor_control Motor1(p23,p24);
+   motor_control Motor2(p25,p26);
+   motor_control *M = &Motor1;
+   float motor_percentage = -1, motor_direction = 0.5, motor_strength = 0; //motor direction is a float from 0-1 which indicates the relative strength of one motor to the other the strength is the PWM output of the strongest motor as dictated by direction
+   direction PREV_DIR = CLOCKWISE;
+   
+   
+   
+   
+   char* pEND, *pEND2;
+   
+//MOT_channel      
+   for (;;)
+   {
+       
+       //OS::message<byte> MOTOR_message;
+       for(byte i = 0; i<100; i++)
+       {
+           MOT_channel.pop(Motor_command[i]); //if data is available, pop, else wait
+            leds = 0x5;
+            if(Motor_command[i] == '\0')  //keep popping chars until a delimiter is found
+                break;
+            else if(Motor_command[i] != '\0' && i == 99)
+                Motor_command[i] = '\0';
+                
+       }
+        
+        //sprintf(MOT_Return_chars, "a command was received.\n"); 
+       //TX_Channel.write(MOT_Return_chars, strlen(MOT_Return_chars)+1); //output the command for debugging
+       //TX_Channel.write(Motor_command, strlen(Motor_command) +1);
+            
+       //which motor?
+       //if (strlen(Servo_Command) >= 20 && strstr(Servo_Command,":Calibrate")) 
+        if(strstr(Motor_command, "MOT:") != NULL)
+           {
+           Motor_number = strtod(1+strstr(Motor_command, ":"), &pEND);
+           // motor_percentage = -1;
+          
+           //sprintf(output_chars, "motor number is: %d\n", Motor_number);
+           //TX_Channel.write(output_chars, strlen(output_chars)+1); //output the command for debugging
+           
+           
+               if(Motor_number < 2) // valid conversion
+                    {
+                        if(Motor_number == 0)
+                            M = &Motor1;
+                        else
+                            M = &Motor2;
+                        motor_percentage = strtod(1+pEND, &pEND2);
+                        //sprintf(output_chars, "percentage is: %f\n", motor_percentage);
+                        //TX_Channel.write(output_chars, strlen(output_chars)+1); //output the command for debugging
+                        M->set_speed(motor_percentage*(-1)); 
+                    } //end valid conversion check
+                } //end single MOT: command
+                
+                
+                else if(strstr(Motor_command, "MO2:") != NULL)
+                {
+                    motor_direction = strtod(&Motor_command[4], &pEND);
+                    if(motor_direction >= -1 && motor_direction <= 1)
+                    {                    
+                        motor_strength = strtod(1+pEND, &pEND2);
+                        if(motor_strength >= -1 && motor_strength <= 1)
+                        {
+                            //sprintf(output_chars, "Motor direction is: %f Motor strength is:%f\n", motor_direction, motor_strength);
+                            //TX_Channel.write(output_chars, strlen(output_chars)+1); //output the command for debugging
+                            if(motor_direction < 0)
+                            {
+                                Motor1.set_speed(motor_strength);
+                                Motor2.set_speed(motor_strength*abs(motor_direction)*2);
+                            }
+                            else if(motor_direction>0)
+                            {
+                                Motor2.set_speed(motor_strength);
+                                Motor1.set_speed(motor_strength*abs(motor_direction)*2);
+                            
+                            }
+                        }
+                    }
+                
+           }
+           leds = 0x5;
+   }
+   
+   
+    
+}  
+
+
+
+#endif
+