File content as of revision 0:9b057566f9ee:

#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