Program to control an accelerometer, motors and a rangefinder using the ScmRTOS ported to mbed. (Work in progress and buggy)
Hexacopter/Motor_control.h@0:9b057566f9ee, 2010-11-01 (annotated)
- Committer:
- jberry
- Date:
- Mon Nov 01 20:39:01 2010 +0000
- Revision:
- 0:9b057566f9ee
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jberry | 0:9b057566f9ee | 1 | #pragma once |
jberry | 0:9b057566f9ee | 2 | #ifndef MOTOR_CONTROL_H |
jberry | 0:9b057566f9ee | 3 | #define MOTOR_CONTROL_H |
jberry | 0:9b057566f9ee | 4 | #include <cmath> |
jberry | 0:9b057566f9ee | 5 | #include <processes.h> |
jberry | 0:9b057566f9ee | 6 | |
jberry | 0:9b057566f9ee | 7 | |
jberry | 0:9b057566f9ee | 8 | //Motor_controller M_cont_process; |
jberry | 0:9b057566f9ee | 9 | enum direction{CLOCKWISE,ANTICLOCKWISE}; |
jberry | 0:9b057566f9ee | 10 | //PwmOut Out1(p23); //motor1 |
jberry | 0:9b057566f9ee | 11 | //PwmOut Out2(p24); |
jberry | 0:9b057566f9ee | 12 | //PwmOut Out3(p25); |
jberry | 0:9b057566f9ee | 13 | //PwmOut Out4(p26); |
jberry | 0:9b057566f9ee | 14 | |
jberry | 0:9b057566f9ee | 15 | class motor_control |
jberry | 0:9b057566f9ee | 16 | { |
jberry | 0:9b057566f9ee | 17 | public: |
jberry | 0:9b057566f9ee | 18 | motor_control(PinName OUT1, PinName OUT2) |
jberry | 0:9b057566f9ee | 19 | :Out1(OUT1),Out2(OUT2),Output(&Out1) |
jberry | 0:9b057566f9ee | 20 | { |
jberry | 0:9b057566f9ee | 21 | //Output = &Out1; |
jberry | 0:9b057566f9ee | 22 | set_direction(CLOCKWISE); |
jberry | 0:9b057566f9ee | 23 | SPEED = 0.0; |
jberry | 0:9b057566f9ee | 24 | Out1.pulsewidth_ms(1); // pulsewidth should be 1ms |
jberry | 0:9b057566f9ee | 25 | Out1.write(0.0); //set initial duty cycle to 0 (i.e. off) |
jberry | 0:9b057566f9ee | 26 | Out2.write(0.0); //set initial duty cycle to 0 (i.e. off) |
jberry | 0:9b057566f9ee | 27 | |
jberry | 0:9b057566f9ee | 28 | |
jberry | 0:9b057566f9ee | 29 | |
jberry | 0:9b057566f9ee | 30 | |
jberry | 0:9b057566f9ee | 31 | } |
jberry | 0:9b057566f9ee | 32 | |
jberry | 0:9b057566f9ee | 33 | void set_direction(direction D) |
jberry | 0:9b057566f9ee | 34 | { |
jberry | 0:9b057566f9ee | 35 | if( D == CLOCKWISE ) |
jberry | 0:9b057566f9ee | 36 | { |
jberry | 0:9b057566f9ee | 37 | *Output = 0; |
jberry | 0:9b057566f9ee | 38 | Output = &Out2; |
jberry | 0:9b057566f9ee | 39 | } |
jberry | 0:9b057566f9ee | 40 | else |
jberry | 0:9b057566f9ee | 41 | { |
jberry | 0:9b057566f9ee | 42 | *Output = 0; |
jberry | 0:9b057566f9ee | 43 | Output = &Out1; |
jberry | 0:9b057566f9ee | 44 | } |
jberry | 0:9b057566f9ee | 45 | DIR = D; |
jberry | 0:9b057566f9ee | 46 | //set_speed(get_speed()); |
jberry | 0:9b057566f9ee | 47 | } |
jberry | 0:9b057566f9ee | 48 | |
jberry | 0:9b057566f9ee | 49 | direction get_direction() |
jberry | 0:9b057566f9ee | 50 | { |
jberry | 0:9b057566f9ee | 51 | return DIR; |
jberry | 0:9b057566f9ee | 52 | } |
jberry | 0:9b057566f9ee | 53 | |
jberry | 0:9b057566f9ee | 54 | void set_speed(float speed) |
jberry | 0:9b057566f9ee | 55 | { |
jberry | 0:9b057566f9ee | 56 | SPEED = speed; |
jberry | 0:9b057566f9ee | 57 | if(speed >= -1 && speed < 0) |
jberry | 0:9b057566f9ee | 58 | { |
jberry | 0:9b057566f9ee | 59 | if(get_direction() == CLOCKWISE) //ALLOW SOME TIME for the motor to change direction |
jberry | 0:9b057566f9ee | 60 | { |
jberry | 0:9b057566f9ee | 61 | *Output = 0; |
jberry | 0:9b057566f9ee | 62 | //wait_ms(500); |
jberry | 0:9b057566f9ee | 63 | } |
jberry | 0:9b057566f9ee | 64 | |
jberry | 0:9b057566f9ee | 65 | set_direction(ANTICLOCKWISE); |
jberry | 0:9b057566f9ee | 66 | *Output = speed; |
jberry | 0:9b057566f9ee | 67 | } |
jberry | 0:9b057566f9ee | 68 | else if(speed >= 0 && speed <= 1) |
jberry | 0:9b057566f9ee | 69 | { |
jberry | 0:9b057566f9ee | 70 | if(get_direction() == CLOCKWISE) //ALLOW SOME TIME for the motor to change direction |
jberry | 0:9b057566f9ee | 71 | { |
jberry | 0:9b057566f9ee | 72 | *Output = 0; |
jberry | 0:9b057566f9ee | 73 | //wait_ms(500); |
jberry | 0:9b057566f9ee | 74 | } |
jberry | 0:9b057566f9ee | 75 | |
jberry | 0:9b057566f9ee | 76 | set_direction(CLOCKWISE); |
jberry | 0:9b057566f9ee | 77 | *Output = speed; |
jberry | 0:9b057566f9ee | 78 | } |
jberry | 0:9b057566f9ee | 79 | |
jberry | 0:9b057566f9ee | 80 | } |
jberry | 0:9b057566f9ee | 81 | float get_speed() |
jberry | 0:9b057566f9ee | 82 | { |
jberry | 0:9b057566f9ee | 83 | return SPEED; |
jberry | 0:9b057566f9ee | 84 | } |
jberry | 0:9b057566f9ee | 85 | |
jberry | 0:9b057566f9ee | 86 | void speed_inc() |
jberry | 0:9b057566f9ee | 87 | { |
jberry | 0:9b057566f9ee | 88 | if( (get_speed() + 0.1) <= 1) |
jberry | 0:9b057566f9ee | 89 | set_speed(SPEED + 0.1); |
jberry | 0:9b057566f9ee | 90 | else if( get_speed() + 0.1 > 1) |
jberry | 0:9b057566f9ee | 91 | set_speed(1); |
jberry | 0:9b057566f9ee | 92 | } |
jberry | 0:9b057566f9ee | 93 | |
jberry | 0:9b057566f9ee | 94 | void speed_dec() |
jberry | 0:9b057566f9ee | 95 | { |
jberry | 0:9b057566f9ee | 96 | if( (get_speed() - 0.1) >= 0) |
jberry | 0:9b057566f9ee | 97 | set_speed(SPEED - 0.1); |
jberry | 0:9b057566f9ee | 98 | else if( get_speed() - 0.1 < 0) |
jberry | 0:9b057566f9ee | 99 | set_speed(0); |
jberry | 0:9b057566f9ee | 100 | |
jberry | 0:9b057566f9ee | 101 | } |
jberry | 0:9b057566f9ee | 102 | |
jberry | 0:9b057566f9ee | 103 | void change_direction() |
jberry | 0:9b057566f9ee | 104 | { |
jberry | 0:9b057566f9ee | 105 | if (get_direction() == CLOCKWISE) |
jberry | 0:9b057566f9ee | 106 | set_direction(ANTICLOCKWISE); |
jberry | 0:9b057566f9ee | 107 | else |
jberry | 0:9b057566f9ee | 108 | set_direction(CLOCKWISE); |
jberry | 0:9b057566f9ee | 109 | } |
jberry | 0:9b057566f9ee | 110 | |
jberry | 0:9b057566f9ee | 111 | |
jberry | 0:9b057566f9ee | 112 | private: |
jberry | 0:9b057566f9ee | 113 | //int PIN1,PIN2; |
jberry | 0:9b057566f9ee | 114 | direction DIR; |
jberry | 0:9b057566f9ee | 115 | PwmOut Out1; |
jberry | 0:9b057566f9ee | 116 | PwmOut Out2; |
jberry | 0:9b057566f9ee | 117 | PwmOut* Output; |
jberry | 0:9b057566f9ee | 118 | |
jberry | 0:9b057566f9ee | 119 | |
jberry | 0:9b057566f9ee | 120 | float SPEED; |
jberry | 0:9b057566f9ee | 121 | |
jberry | 0:9b057566f9ee | 122 | }; |
jberry | 0:9b057566f9ee | 123 | |
jberry | 0:9b057566f9ee | 124 | |
jberry | 0:9b057566f9ee | 125 | char MOT_Return_chars[100]; //defined in accelerometer.h |
jberry | 0:9b057566f9ee | 126 | char Motor_command[100] = ""; |
jberry | 0:9b057566f9ee | 127 | byte Motor_number = 0; |
jberry | 0:9b057566f9ee | 128 | |
jberry | 0:9b057566f9ee | 129 | extern BusOut leds; |
jberry | 0:9b057566f9ee | 130 | template<> OS_PROCESS void Motor_controller::Exec() //motor control handling process |
jberry | 0:9b057566f9ee | 131 | { |
jberry | 0:9b057566f9ee | 132 | Motor_command[99] = '\0'; |
jberry | 0:9b057566f9ee | 133 | motor_control Motor1(p23,p24); |
jberry | 0:9b057566f9ee | 134 | motor_control Motor2(p25,p26); |
jberry | 0:9b057566f9ee | 135 | motor_control *M = &Motor1; |
jberry | 0:9b057566f9ee | 136 | 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 |
jberry | 0:9b057566f9ee | 137 | direction PREV_DIR = CLOCKWISE; |
jberry | 0:9b057566f9ee | 138 | |
jberry | 0:9b057566f9ee | 139 | |
jberry | 0:9b057566f9ee | 140 | |
jberry | 0:9b057566f9ee | 141 | |
jberry | 0:9b057566f9ee | 142 | char* pEND, *pEND2; |
jberry | 0:9b057566f9ee | 143 | |
jberry | 0:9b057566f9ee | 144 | //MOT_channel |
jberry | 0:9b057566f9ee | 145 | for (;;) |
jberry | 0:9b057566f9ee | 146 | { |
jberry | 0:9b057566f9ee | 147 | |
jberry | 0:9b057566f9ee | 148 | //OS::message<byte> MOTOR_message; |
jberry | 0:9b057566f9ee | 149 | for(byte i = 0; i<100; i++) |
jberry | 0:9b057566f9ee | 150 | { |
jberry | 0:9b057566f9ee | 151 | MOT_channel.pop(Motor_command[i]); //if data is available, pop, else wait |
jberry | 0:9b057566f9ee | 152 | leds = 0x5; |
jberry | 0:9b057566f9ee | 153 | if(Motor_command[i] == '\0') //keep popping chars until a delimiter is found |
jberry | 0:9b057566f9ee | 154 | break; |
jberry | 0:9b057566f9ee | 155 | else if(Motor_command[i] != '\0' && i == 99) |
jberry | 0:9b057566f9ee | 156 | Motor_command[i] = '\0'; |
jberry | 0:9b057566f9ee | 157 | |
jberry | 0:9b057566f9ee | 158 | } |
jberry | 0:9b057566f9ee | 159 | |
jberry | 0:9b057566f9ee | 160 | //sprintf(MOT_Return_chars, "a command was received.\n"); |
jberry | 0:9b057566f9ee | 161 | //TX_Channel.write(MOT_Return_chars, strlen(MOT_Return_chars)+1); //output the command for debugging |
jberry | 0:9b057566f9ee | 162 | //TX_Channel.write(Motor_command, strlen(Motor_command) +1); |
jberry | 0:9b057566f9ee | 163 | |
jberry | 0:9b057566f9ee | 164 | //which motor? |
jberry | 0:9b057566f9ee | 165 | //if (strlen(Servo_Command) >= 20 && strstr(Servo_Command,":Calibrate")) |
jberry | 0:9b057566f9ee | 166 | if(strstr(Motor_command, "MOT:") != NULL) |
jberry | 0:9b057566f9ee | 167 | { |
jberry | 0:9b057566f9ee | 168 | Motor_number = strtod(1+strstr(Motor_command, ":"), &pEND); |
jberry | 0:9b057566f9ee | 169 | // motor_percentage = -1; |
jberry | 0:9b057566f9ee | 170 | |
jberry | 0:9b057566f9ee | 171 | //sprintf(output_chars, "motor number is: %d\n", Motor_number); |
jberry | 0:9b057566f9ee | 172 | //TX_Channel.write(output_chars, strlen(output_chars)+1); //output the command for debugging |
jberry | 0:9b057566f9ee | 173 | |
jberry | 0:9b057566f9ee | 174 | |
jberry | 0:9b057566f9ee | 175 | if(Motor_number < 2) // valid conversion |
jberry | 0:9b057566f9ee | 176 | { |
jberry | 0:9b057566f9ee | 177 | if(Motor_number == 0) |
jberry | 0:9b057566f9ee | 178 | M = &Motor1; |
jberry | 0:9b057566f9ee | 179 | else |
jberry | 0:9b057566f9ee | 180 | M = &Motor2; |
jberry | 0:9b057566f9ee | 181 | motor_percentage = strtod(1+pEND, &pEND2); |
jberry | 0:9b057566f9ee | 182 | //sprintf(output_chars, "percentage is: %f\n", motor_percentage); |
jberry | 0:9b057566f9ee | 183 | //TX_Channel.write(output_chars, strlen(output_chars)+1); //output the command for debugging |
jberry | 0:9b057566f9ee | 184 | M->set_speed(motor_percentage*(-1)); |
jberry | 0:9b057566f9ee | 185 | } //end valid conversion check |
jberry | 0:9b057566f9ee | 186 | } //end single MOT: command |
jberry | 0:9b057566f9ee | 187 | |
jberry | 0:9b057566f9ee | 188 | |
jberry | 0:9b057566f9ee | 189 | else if(strstr(Motor_command, "MO2:") != NULL) |
jberry | 0:9b057566f9ee | 190 | { |
jberry | 0:9b057566f9ee | 191 | motor_direction = strtod(&Motor_command[4], &pEND); |
jberry | 0:9b057566f9ee | 192 | if(motor_direction >= -1 && motor_direction <= 1) |
jberry | 0:9b057566f9ee | 193 | { |
jberry | 0:9b057566f9ee | 194 | motor_strength = strtod(1+pEND, &pEND2); |
jberry | 0:9b057566f9ee | 195 | if(motor_strength >= -1 && motor_strength <= 1) |
jberry | 0:9b057566f9ee | 196 | { |
jberry | 0:9b057566f9ee | 197 | //sprintf(output_chars, "Motor direction is: %f Motor strength is:%f\n", motor_direction, motor_strength); |
jberry | 0:9b057566f9ee | 198 | //TX_Channel.write(output_chars, strlen(output_chars)+1); //output the command for debugging |
jberry | 0:9b057566f9ee | 199 | if(motor_direction < 0) |
jberry | 0:9b057566f9ee | 200 | { |
jberry | 0:9b057566f9ee | 201 | Motor1.set_speed(motor_strength); |
jberry | 0:9b057566f9ee | 202 | Motor2.set_speed(motor_strength*abs(motor_direction)*2); |
jberry | 0:9b057566f9ee | 203 | } |
jberry | 0:9b057566f9ee | 204 | else if(motor_direction>0) |
jberry | 0:9b057566f9ee | 205 | { |
jberry | 0:9b057566f9ee | 206 | Motor2.set_speed(motor_strength); |
jberry | 0:9b057566f9ee | 207 | Motor1.set_speed(motor_strength*abs(motor_direction)*2); |
jberry | 0:9b057566f9ee | 208 | |
jberry | 0:9b057566f9ee | 209 | } |
jberry | 0:9b057566f9ee | 210 | } |
jberry | 0:9b057566f9ee | 211 | } |
jberry | 0:9b057566f9ee | 212 | |
jberry | 0:9b057566f9ee | 213 | } |
jberry | 0:9b057566f9ee | 214 | leds = 0x5; |
jberry | 0:9b057566f9ee | 215 | } |
jberry | 0:9b057566f9ee | 216 | |
jberry | 0:9b057566f9ee | 217 | |
jberry | 0:9b057566f9ee | 218 | |
jberry | 0:9b057566f9ee | 219 | } |
jberry | 0:9b057566f9ee | 220 | |
jberry | 0:9b057566f9ee | 221 | |
jberry | 0:9b057566f9ee | 222 | |
jberry | 0:9b057566f9ee | 223 | #endif |
jberry | 0:9b057566f9ee | 224 |