Program to control an accelerometer, motors and a rangefinder using the ScmRTOS ported to mbed. (Work in progress and buggy)
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 +