Proto Drive
Dependencies: RPCInterface mbed
measurement.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 ISENSOR_SLOPE 23.9 //slope of the current sensor in [A/V] |
| protodriveev | 0:2dbd366be5fd | 6 | #define Y_INTERCEPT -60.1563 //sensor voltage at 0 current |
| protodriveev | 0:2dbd366be5fd | 7 | #define NUM_HALF_CYCLES 4 //set the number of ticks over which to measure speed |
| protodriveev | 0:2dbd366be5fd | 8 | #define WAIT_BEFORE_SPEEDSTOP 7.5 //amount of time to wait before declaring that the motor is not moving. 7.5s corresponds to a speed of 1 rev/min |
| protodriveev | 0:2dbd366be5fd | 9 | #define R1 200000//voltage measurement resistor connected to device under measurement [Ohm] |
| protodriveev | 0:2dbd366be5fd | 10 | #define R2 100000//voltage measurement resistor connected to ground [Ohm] |
| protodriveev | 0:2dbd366be5fd | 11 | |
| protodriveev | 0:2dbd366be5fd | 12 | /***********************************************************/ |
| protodriveev | 0:2dbd366be5fd | 13 | /*Variables */ |
| protodriveev | 0:2dbd366be5fd | 14 | /***********************************************************/ |
| protodriveev | 0:2dbd366be5fd | 15 | |
| protodriveev | 0:2dbd366be5fd | 16 | float end, begin, current_read; |
| protodriveev | 0:2dbd366be5fd | 17 | int current_state; |
| protodriveev | 0:2dbd366be5fd | 18 | int count = 0; |
| protodriveev | 0:2dbd366be5fd | 19 | |
| protodriveev | 0:2dbd366be5fd | 20 | Timer speed_timer; |
| protodriveev | 0:2dbd366be5fd | 21 | |
| protodriveev | 0:2dbd366be5fd | 22 | /***********************************************************/ |
| protodriveev | 0:2dbd366be5fd | 23 | /*Pin setup */ |
| protodriveev | 0:2dbd366be5fd | 24 | /***********************************************************/ |
| protodriveev | 0:2dbd366be5fd | 25 | //Encoder |
| protodriveev | 0:2dbd366be5fd | 26 | DigitalIn speed_yellow(p7); //yellow wire from sinusoidal encoder |
| protodriveev | 0:2dbd366be5fd | 27 | DigitalIn speed_green(p8); //green wire from sinusoidal encoder |
| protodriveev | 0:2dbd366be5fd | 28 | |
| protodriveev | 0:2dbd366be5fd | 29 | /***********************************************************/ |
| protodriveev | 0:2dbd366be5fd | 30 | /*Subroutines */ |
| protodriveev | 0:2dbd366be5fd | 31 | /***********************************************************/ |
| protodriveev | 0:2dbd366be5fd | 32 | |
| protodriveev | 0:2dbd366be5fd | 33 | //get the voltage for one of the energy storage devices. Takes pin as a parameter |
| protodriveev | 0:2dbd366be5fd | 34 | float get_voltage (AnalogIn& pin) { |
| protodriveev | 0:2dbd366be5fd | 35 | float voltage; |
| protodriveev | 0:2dbd366be5fd | 36 | voltage = pin.read()*3.3*(R1+R2)/R2; //scaling to account for voltage divider |
| protodriveev | 0:2dbd366be5fd | 37 | return voltage; |
| protodriveev | 0:2dbd366be5fd | 38 | } |
| protodriveev | 0:2dbd366be5fd | 39 | |
| protodriveev | 0:2dbd366be5fd | 40 | //returns current in amps |
| protodriveev | 0:2dbd366be5fd | 41 | #ifdef CURRENT_SENSOR_ON |
| protodriveev | 0:2dbd366be5fd | 42 | void get_current() { |
| protodriveev | 0:2dbd366be5fd | 43 | current_read = current_sense.read(); //read raw AnalogIn value of current |
| protodriveev | 0:2dbd366be5fd | 44 | current = ISENSOR_SLOPE*current_read*3.3 + Y_INTERCEPT; //scaling to get current in A |
| protodriveev | 0:2dbd366be5fd | 45 | } |
| protodriveev | 0:2dbd366be5fd | 46 | #endif |
| protodriveev | 0:2dbd366be5fd | 47 | |
| protodriveev | 0:2dbd366be5fd | 48 | //returns speed in rad/sec |
| protodriveev | 0:2dbd366be5fd | 49 | void get_speed() { |
| protodriveev | 0:2dbd366be5fd | 50 | current_state = speed_yellow; //get the current state of speed_yellow pin (0 or 1) |
| protodriveev | 0:2dbd366be5fd | 51 | speed_timer.start(); |
| protodriveev | 0:2dbd366be5fd | 52 | while (speed_yellow == current_state && speed_timer <=WAIT_BEFORE_SPEEDSTOP) {} //wait for value of the speed_yellow to change, indicating the beginning of a new cycle |
| protodriveev | 0:2dbd366be5fd | 53 | if (speed_timer < WAIT_BEFORE_SPEEDSTOP) { //check that the timer is less than WAIT_BEFORE_SPEEDSTOP, to make sure it has not been running for too long. This will happen if speed = 0 |
| protodriveev | 0:2dbd366be5fd | 54 | speed_timer.reset(); //reset the timer so that it starts timing from the beginning of the new cycle |
| protodriveev | 0:2dbd366be5fd | 55 | begin = speed_timer.read(); |
| protodriveev | 0:2dbd366be5fd | 56 | for (int i = 1; i <= NUM_HALF_CYCLES; i++) { //loop to allow timing over a set number of encoder cycles |
| protodriveev | 0:2dbd366be5fd | 57 | current_state = speed_yellow; |
| protodriveev | 0:2dbd366be5fd | 58 | while (speed_yellow == current_state && speed_timer <= WAIT_BEFORE_SPEEDSTOP) {}//wait for speed_yellow pin to change. If it does not change, the loop will exit when speed_timer = WAIT_BEFORE_SPEEDSTOP |
| protodriveev | 0:2dbd366be5fd | 59 | } |
| protodriveev | 0:2dbd366be5fd | 60 | if (speed_timer < WAIT_BEFORE_SPEEDSTOP) { |
| protodriveev | 0:2dbd366be5fd | 61 | end = speed_timer.read(); //time at the end of timing NUM_HALF_CYCLES cycles |
| protodriveev | 0:2dbd366be5fd | 62 | speed_timer.stop(); |
| protodriveev | 0:2dbd366be5fd | 63 | speed =((3.14159265/8)*NUM_HALF_CYCLES)/(end-begin); //record speed in rad/sec |
| protodriveev | 0:2dbd366be5fd | 64 | } else { |
| protodriveev | 0:2dbd366be5fd | 65 | speed = 0; //speed = 0 if the timer has exceeded WAIT_BEFORE_SPEEDSTOP |
| protodriveev | 0:2dbd366be5fd | 66 | } |
| protodriveev | 0:2dbd366be5fd | 67 | } else { |
| protodriveev | 0:2dbd366be5fd | 68 | speed = 0; //speed = 0 if the timer has exceeded WAIT_BEFORE_SPEEDSTOP |
| protodriveev | 0:2dbd366be5fd | 69 | } |
| protodriveev | 0:2dbd366be5fd | 70 | } |