Proto Drive
Dependencies: RPCInterface mbed
motor_control.h@0:2dbd366be5fd, 2012-05-07 (annotated)
- Committer:
- protodriveev
- Date:
- Mon May 07 16:14:45 2012 +0000
- Revision:
- 0:2dbd366be5fd
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| protodriveev | 0:2dbd366be5fd | 1 | /***********************************************************/ |
| protodriveev | 0:2dbd366be5fd | 2 | /*Constants */ |
| protodriveev | 0:2dbd366be5fd | 3 | /***********************************************************/ |
| protodriveev | 0:2dbd366be5fd | 4 | |
| protodriveev | 0:2dbd366be5fd | 5 | #define V_MOTOR_RATED 6 //rated motor voltage [V]. this must be set to be less than the voltage of the batteries |
| protodriveev | 0:2dbd366be5fd | 6 | #define PERIOD_US 25 //define the PWM period in microseconds, currently set for 40kHz |
| protodriveev | 0:2dbd366be5fd | 7 | |
| protodriveev | 0:2dbd366be5fd | 8 | /***********************************************************/ |
| protodriveev | 0:2dbd366be5fd | 9 | /*Pin setup */ |
| protodriveev | 0:2dbd366be5fd | 10 | /***********************************************************/ |
| protodriveev | 0:2dbd366be5fd | 11 | |
| protodriveev | 0:2dbd366be5fd | 12 | //Enable PWM inout to motorcontroller |
| protodriveev | 0:2dbd366be5fd | 13 | PwmOut DUT_motor(p22); |
| protodriveev | 0:2dbd366be5fd | 14 | PwmOut load_motor(p21); |
| protodriveev | 0:2dbd366be5fd | 15 | |
| protodriveev | 0:2dbd366be5fd | 16 | //Motor direction control on motor controller switch 2 |
| protodriveev | 0:2dbd366be5fd | 17 | DigitalOut DUT_direction(p5); |
| protodriveev | 0:2dbd366be5fd | 18 | DigitalOut load_direction(p6); |
| protodriveev | 0:2dbd366be5fd | 19 | |
| protodriveev | 0:2dbd366be5fd | 20 | /***********************************************************/ |
| protodriveev | 0:2dbd366be5fd | 21 | /*Subroutines */ |
| protodriveev | 0:2dbd366be5fd | 22 | /***********************************************************/ |
| protodriveev | 0:2dbd366be5fd | 23 | |
| protodriveev | 0:2dbd366be5fd | 24 | //initialize PWM |
| protodriveev | 0:2dbd366be5fd | 25 | void init_pwm () { |
| protodriveev | 0:2dbd366be5fd | 26 | //set the PWM channel periods |
| protodriveev | 0:2dbd366be5fd | 27 | DUT_motor.period_us(PERIOD_US); |
| protodriveev | 0:2dbd366be5fd | 28 | load_motor.period_us(PERIOD_US); |
| protodriveev | 0:2dbd366be5fd | 29 | |
| protodriveev | 0:2dbd366be5fd | 30 | //initialize duty cycle to 0 to make sure motors are off |
| protodriveev | 0:2dbd366be5fd | 31 | DUT_motor =0; |
| protodriveev | 0:2dbd366be5fd | 32 | load_motor =0; |
| protodriveev | 0:2dbd366be5fd | 33 | } |
| protodriveev | 0:2dbd366be5fd | 34 | |
| protodriveev | 0:2dbd366be5fd | 35 | /* |
| protodriveev | 0:2dbd366be5fd | 36 | Set PWM duty cycle |
| protodriveev | 0:2dbd366be5fd | 37 | - Motors are connected so that they will always oppose each other. |
| protodriveev | 0:2dbd366be5fd | 38 | - Duty cycle (a float from 0-1) determines the voltage applied to the motor terminals as a percentage of the rated voltage. eg. if duty is 0.5 and rated voltage is 6V, then terminal voltage will be 3V |
| protodriveev | 0:2dbd366be5fd | 39 | */ |
| protodriveev | 0:2dbd366be5fd | 40 | void set_duty (float DUT_demand, float load_demand) { |
| protodriveev | 0:2dbd366be5fd | 41 | |
| protodriveev | 0:2dbd366be5fd | 42 | //DUT batt and cap enabled |
| protodriveev | 0:2dbd366be5fd | 43 | #ifdef DUT_BATT_CAP_ENABLE |
| protodriveev | 0:2dbd366be5fd | 44 | if (relay) { |
| protodriveev | 0:2dbd366be5fd | 45 | v_DUT_batt = get_voltage(v_DUT_batt_measure); //get voltage of DUT battery |
| protodriveev | 0:2dbd366be5fd | 46 | DUT_demand = DUT_demand*(V_MOTOR_RATED/v_DUT_batt); //scale the demand so that the voltage to the motor does not exceed its rated max |
| protodriveev | 0:2dbd366be5fd | 47 | } else if (relay == 0) { //if cap is currently being used |
| protodriveev | 0:2dbd366be5fd | 48 | v_cap = get_voltage(v_cap_measure); //get voltage of cap |
| protodriveev | 0:2dbd366be5fd | 49 | DUT_demand = DUT_demand*(V_MOTOR_RATED/v_cap); //scale the demand so that the voltage to the motor does not exceed its rated max |
| protodriveev | 0:2dbd366be5fd | 50 | } else { |
| protodriveev | 0:2dbd366be5fd | 51 | big_error = 1; //error flag |
| protodriveev | 0:2dbd366be5fd | 52 | big_error_led = 1; //turn on error LED |
| protodriveev | 0:2dbd366be5fd | 53 | } |
| protodriveev | 0:2dbd366be5fd | 54 | DUT_motor.write(DUT_demand); //write new DUT duty cycle |
| protodriveev | 0:2dbd366be5fd | 55 | #endif |
| protodriveev | 0:2dbd366be5fd | 56 | |
| protodriveev | 0:2dbd366be5fd | 57 | //DUT batt connected |
| protodriveev | 0:2dbd366be5fd | 58 | #ifdef DUT_BATT_ENABLE |
| protodriveev | 0:2dbd366be5fd | 59 | v_DUT_batt = get_voltage(v_DUT_batt_measure); //get voltage of DUT battery |
| protodriveev | 0:2dbd366be5fd | 60 | DUT_demand = DUT_demand*(V_MOTOR_RATED/v_DUT_batt); //scale the demand so that the voltage to the motor does not exceed its rated max |
| protodriveev | 0:2dbd366be5fd | 61 | DUT_motor.write(DUT_demand); //change DUT duty cycle based on demand |
| protodriveev | 0:2dbd366be5fd | 62 | #endif |
| protodriveev | 0:2dbd366be5fd | 63 | |
| protodriveev | 0:2dbd366be5fd | 64 | //Load batt connected |
| protodriveev | 0:2dbd366be5fd | 65 | #ifdef LOAD_BATT_ENABLE |
| protodriveev | 0:2dbd366be5fd | 66 | v_load_batt = get_voltage(v_load_batt_measure); //get voltage of load battery |
| protodriveev | 0:2dbd366be5fd | 67 | load_demand = load_demand*(V_MOTOR_RATED/v_load_batt); //scale the demand so that the voltage to the motor does not exceed its rated max |
| protodriveev | 0:2dbd366be5fd | 68 | load_motor.write(load_demand); //change load duty cycle based on demand |
| protodriveev | 0:2dbd366be5fd | 69 | #endif |
| protodriveev | 0:2dbd366be5fd | 70 | |
| protodriveev | 0:2dbd366be5fd | 71 | //External Power |
| protodriveev | 0:2dbd366be5fd | 72 | #ifdef DUT_EXTERNAL_POWER_ENABLE |
| protodriveev | 0:2dbd366be5fd | 73 | DUT_motor.write(DUT_demand); //change DUT duty cycle based on demand |
| protodriveev | 0:2dbd366be5fd | 74 | #endif |
| protodriveev | 0:2dbd366be5fd | 75 | |
| protodriveev | 0:2dbd366be5fd | 76 | #ifdef LOAD_EXTERNAL_POWER_ENABLE |
| protodriveev | 0:2dbd366be5fd | 77 | load_motor.write(load_demand); //change load duty cycle based on demand |
| protodriveev | 0:2dbd366be5fd | 78 | #endif |
| protodriveev | 0:2dbd366be5fd | 79 | |
| protodriveev | 0:2dbd366be5fd | 80 | //set motor directions |
| protodriveev | 0:2dbd366be5fd | 81 | DUT_direction.write(DUT_input_direc); |
| protodriveev | 0:2dbd366be5fd | 82 | load_direction.write(load_input_direc); |
| protodriveev | 0:2dbd366be5fd | 83 | } |
| protodriveev | 0:2dbd366be5fd | 84 | |
| protodriveev | 0:2dbd366be5fd | 85 | |
| protodriveev | 0:2dbd366be5fd | 86 | |
| protodriveev | 0:2dbd366be5fd | 87 | |
| protodriveev | 0:2dbd366be5fd | 88 | |
| protodriveev | 0:2dbd366be5fd | 89 |