File content as of revision 0:2dbd366be5fd:

/***********************************************************/
/*Constants                                                */
/***********************************************************/

#define ISENSOR_SLOPE 23.9 //slope of the current sensor in [A/V]
#define Y_INTERCEPT -60.1563 //sensor voltage at 0 current
#define NUM_HALF_CYCLES 4 //set the number of ticks over which to measure speed
#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
#define R1 200000//voltage measurement resistor connected to device under measurement [Ohm]
#define R2 100000//voltage measurement resistor connected to ground [Ohm]

/***********************************************************/
/*Variables                                                */
/***********************************************************/

float end, begin, current_read;
int current_state;
int count = 0;

Timer speed_timer;

/***********************************************************/
/*Pin setup                                                */
/***********************************************************/
//Encoder
DigitalIn speed_yellow(p7); //yellow wire from sinusoidal encoder
DigitalIn speed_green(p8); //green wire from sinusoidal encoder

/***********************************************************/
/*Subroutines                                              */
/***********************************************************/

//get the voltage for one of the energy storage devices. Takes pin as a parameter
float get_voltage (AnalogIn& pin) {
    float voltage;
    voltage = pin.read()*3.3*(R1+R2)/R2; //scaling to account for voltage divider
    return voltage;
}

//returns current in amps
#ifdef CURRENT_SENSOR_ON
void get_current() {
    current_read = current_sense.read(); //read raw AnalogIn value of current
    current = ISENSOR_SLOPE*current_read*3.3 + Y_INTERCEPT; //scaling to get current in A
}
#endif

//returns speed in rad/sec
void get_speed() {
    current_state = speed_yellow; //get the current state of speed_yellow pin (0 or 1)
    speed_timer.start();
    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
    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
        speed_timer.reset(); //reset the timer so that it starts timing from the beginning of the new cycle
        begin = speed_timer.read();
        for (int i = 1; i <= NUM_HALF_CYCLES; i++) { //loop to allow timing over a set number of encoder cycles
            current_state = speed_yellow;
            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
        }
        if (speed_timer < WAIT_BEFORE_SPEEDSTOP) {
            end = speed_timer.read(); //time at the end of timing NUM_HALF_CYCLES cycles
            speed_timer.stop();
            speed =((3.14159265/8)*NUM_HALF_CYCLES)/(end-begin); //record speed in rad/sec
        } else {
            speed = 0; //speed = 0 if the timer has exceeded WAIT_BEFORE_SPEEDSTOP
        }
    } else {
        speed = 0;  //speed = 0 if the timer has exceeded WAIT_BEFORE_SPEEDSTOP
    }
}