Motor_control.h

00001 #pragma once
00002 #ifndef MOTOR_CONTROL_H
00003 #define MOTOR_CONTROL_H
00004 #include <cmath>
00005 #include <processes.h>
00006 
00007 
00008 //Motor_controller M_cont_process;
00009 enum direction{CLOCKWISE,ANTICLOCKWISE};
00010  //PwmOut Out1(p23); //motor1
00011  //PwmOut Out2(p24);
00012  //PwmOut Out3(p25);
00013  //PwmOut Out4(p26);
00014 
00015 class motor_control
00016 {
00017     public:
00018     motor_control(PinName OUT1, PinName OUT2)
00019     :Out1(OUT1),Out2(OUT2),Output(&Out1)
00020     {
00021         //Output = &Out1;
00022         set_direction(CLOCKWISE);
00023         SPEED = 0.0;
00024         Out1.pulsewidth_ms(1); // pulsewidth should be 1ms
00025         Out1.write(0.0); //set initial duty cycle to 0 (i.e. off)
00026         Out2.write(0.0); //set initial duty cycle to 0 (i.e. off)
00027         
00028         
00029 
00030         
00031      }
00032         
00033     void set_direction(direction D)
00034     {
00035         if( D == CLOCKWISE )
00036         {
00037             *Output = 0;
00038             Output = &Out2;            
00039          }
00040         else
00041         {
00042             *Output = 0;
00043             Output = &Out1;            
00044         }
00045         DIR = D;
00046         //set_speed(get_speed());
00047      }
00048     
00049      direction get_direction()
00050      {
00051             return DIR;
00052      }
00053      
00054      void set_speed(float speed)
00055      {
00056         SPEED = speed;
00057         if(speed >= -1 && speed < 0)
00058         {
00059            if(get_direction() == CLOCKWISE) //ALLOW SOME TIME for the motor to change direction
00060            {
00061                 *Output = 0;
00062                 //wait_ms(500);
00063            }
00064            
00065            set_direction(ANTICLOCKWISE);
00066            *Output = speed;
00067         }
00068         else if(speed >= 0 && speed <= 1)
00069         {
00070             if(get_direction() == CLOCKWISE) //ALLOW SOME TIME for the motor to change direction
00071             {
00072                 *Output = 0;
00073                 //wait_ms(500);
00074             }
00075             
00076             set_direction(CLOCKWISE);
00077             *Output = speed;
00078         }
00079         
00080       }
00081       float get_speed()
00082       {
00083         return SPEED;
00084       }
00085       
00086       void speed_inc()
00087       {
00088         if( (get_speed() + 0.1) <= 1)
00089         set_speed(SPEED + 0.1);
00090         else if( get_speed() + 0.1 > 1)
00091         set_speed(1);        
00092       }
00093       
00094       void speed_dec()
00095       {
00096         if( (get_speed() - 0.1) >= 0)
00097         set_speed(SPEED - 0.1);
00098         else if( get_speed() - 0.1 < 0)
00099         set_speed(0);        
00100         
00101       }
00102       
00103       void change_direction()
00104       {
00105         if (get_direction() == CLOCKWISE)
00106             set_direction(ANTICLOCKWISE);
00107         else
00108             set_direction(CLOCKWISE);
00109       }
00110             
00111     
00112     private:
00113     //int PIN1,PIN2;
00114     direction DIR;
00115     PwmOut Out1;
00116     PwmOut Out2;
00117     PwmOut* Output;
00118     
00119 
00120     float SPEED;
00121     
00122  };
00123 
00124 
00125 char MOT_Return_chars[100]; //defined in accelerometer.h
00126 char Motor_command[100] = "";
00127 byte Motor_number = 0;
00128 
00129 extern BusOut leds;
00130 template<> OS_PROCESS void Motor_controller::Exec() //motor control handling process
00131 {
00132     Motor_command[99] = '\0';
00133    motor_control Motor1(p23,p24);
00134    motor_control Motor2(p25,p26);
00135    motor_control *M = &Motor1;
00136    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
00137    direction PREV_DIR = CLOCKWISE;
00138    
00139    
00140    
00141    
00142    char* pEND, *pEND2;
00143    
00144 //MOT_channel      
00145    for (;;)
00146    {
00147        
00148        //OS::message<byte> MOTOR_message;
00149        for(byte i = 0; i<100; i++)
00150        {
00151            MOT_channel.pop(Motor_command[i]); //if data is available, pop, else wait
00152             leds = 0x5;
00153             if(Motor_command[i] == '\0')  //keep popping chars until a delimiter is found
00154                 break;
00155             else if(Motor_command[i] != '\0' && i == 99)
00156                 Motor_command[i] = '\0';
00157                 
00158        }
00159         
00160         //sprintf(MOT_Return_chars, "a command was received.\n"); 
00161        //TX_Channel.write(MOT_Return_chars, strlen(MOT_Return_chars)+1); //output the command for debugging
00162        //TX_Channel.write(Motor_command, strlen(Motor_command) +1);
00163             
00164        //which motor?
00165        //if (strlen(Servo_Command) >= 20 && strstr(Servo_Command,":Calibrate")) 
00166         if(strstr(Motor_command, "MOT:") != NULL)
00167            {
00168            Motor_number = strtod(1+strstr(Motor_command, ":"), &pEND);
00169            // motor_percentage = -1;
00170           
00171            //sprintf(output_chars, "motor number is: %d\n", Motor_number);
00172            //TX_Channel.write(output_chars, strlen(output_chars)+1); //output the command for debugging
00173            
00174            
00175                if(Motor_number < 2) // valid conversion
00176                     {
00177                         if(Motor_number == 0)
00178                             M = &Motor1;
00179                         else
00180                             M = &Motor2;
00181                         motor_percentage = strtod(1+pEND, &pEND2);
00182                         //sprintf(output_chars, "percentage is: %f\n", motor_percentage);
00183                         //TX_Channel.write(output_chars, strlen(output_chars)+1); //output the command for debugging
00184                         M->set_speed(motor_percentage*(-1)); 
00185                     } //end valid conversion check
00186                 } //end single MOT: command
00187                 
00188                 
00189                 else if(strstr(Motor_command, "MO2:") != NULL)
00190                 {
00191                     motor_direction = strtod(&Motor_command[4], &pEND);
00192                     if(motor_direction >= -1 && motor_direction <= 1)
00193                     {                    
00194                         motor_strength = strtod(1+pEND, &pEND2);
00195                         if(motor_strength >= -1 && motor_strength <= 1)
00196                         {
00197                             //sprintf(output_chars, "Motor direction is: %f Motor strength is:%f\n", motor_direction, motor_strength);
00198                             //TX_Channel.write(output_chars, strlen(output_chars)+1); //output the command for debugging
00199                             if(motor_direction < 0)
00200                             {
00201                                 Motor1.set_speed(motor_strength);
00202                                 Motor2.set_speed(motor_strength*abs(motor_direction)*2);
00203                             }
00204                             else if(motor_direction>0)
00205                             {
00206                                 Motor2.set_speed(motor_strength);
00207                                 Motor1.set_speed(motor_strength*abs(motor_direction)*2);
00208                             
00209                             }
00210                         }
00211                     }
00212                 
00213            }
00214            leds = 0x5;
00215    }
00216    
00217    
00218     
00219 }  
00220 
00221 
00222 
00223 #endif
00224