File content as of revision 12:6e13c34c4295:

/***
Basic Program Flow:
mbed turns on. initializes.
enters main while loop then first sub while loop where it listens for a go command
mbed recieves go commmand, initializes for data collection, breaks out of first sub while
    enters second sub while. starts collecting and publishing data, listening for stop
mbed hears stop, breaks out of second while loop, returns back to first while loop


Keep in Mind:
Serial communication between mbed matlab sometimes sucks, and often breaks down
Most of the things in this program (and the corresponding matlab one) are meant
to minimize the amount of bad data that gets passed over serial and maintain the
connection.
***/


#include "mbed.h"
#include "time.h"
#include "stdio.h"
#include "ctype.h"

#define PI 3.14159265358979323846


//Just for basic debugging
//User button controls motor speed
//Green LED should turn on while listening to pc for input before starting run
InterruptIn button(USER_BUTTON);
DigitalOut green(LED2);

// Pololu VNH5019 Motor Driver Carrier. https://www.pololu.com/product/1451
PwmOut pwm(D5);     //pwm input on motor controller. do not use D3
DigitalOut a(D2);   //IN_A input on motor controller
DigitalOut b(D4);   //IN_B input on motor controller

//Hook up to Vout on current sensor
//SparkFun Hall-Effect Current Sensor Breakout - ACS712
//https://www.sparkfun.com/products/8882
AnalogIn currentSense(A5);

//For communication with pc through matlab
//Make sure baud rates are equal
Serial pc(USBTX, USBRX, 115200);

const int CPR = 900*4;  // Encoder counts per revolution (900).  Change to match your encoder. x4 for quadrature
const double VREF = 3;  // Microcontroller reference voltage
const float currentSensorOutputRatio = 0.185;   // Volts/Amp specified by current sensor. Divide Voltage by cSenseOutput to get current
const float PSupply_Voltage = 12.0;             // Voltage input from powersupply
const float Output_Voltage = 6.0;               // Maximum output voltage desired
const float pwm_pulley = Output_Voltage/PSupply_Voltage;


///setup code for encoder on ***** pins PA0 and PA1 (A0 and A1) ****** ///
void EncoderInitialise(void) {
    // configure GPIO PA0 & PA1 as inputs for Encoder
    RCC->AHB1ENR |= 0x00000001;  // Enable clock for GPIOA
 
    GPIOA->MODER   |= GPIO_MODER_MODER0_1 | GPIO_MODER_MODER1_1 ;           //PA0 & PA1 as Alternate Function   /*!< GPIO port mode register,               Address offset: 0x00      */
    GPIOA->OTYPER  |= GPIO_OTYPER_OT_0 | GPIO_OTYPER_OT_1 ;                 //PA0 & PA1 as Inputs               /*!< GPIO port output type register,        Address offset: 0x04      */
    GPIOA->OSPEEDR |= 0x00000011;//|= GPIO_OSPEEDER_OSPEEDR0 | GPIO_OSPEEDER_OSPEEDR1 ;     // Low speed                        /*!< GPIO port output speed register,       Address offset: 0x08      */
    GPIOA->PUPDR   |= GPIO_PUPDR_PUPDR0_1 | GPIO_PUPDR_PUPDR1_1 ;           // Pull Down                        /*!< GPIO port pull-up/pull-down register,  Address offset: 0x0C      */
    GPIOA->AFR[0]  |= 0x00000011 ;                                          //  AF01 for PA0 & PA1              /*!< GPIO alternate function registers,     Address offset: 0x20-0x24 */
    GPIOA->AFR[1]  |= 0x00000000 ;                                          //                                  /*!< GPIO alternate function registers,     Address offset: 0x20-0x24 */
   
    // configure TIM2 as Encoder input
    RCC->APB1ENR |= 0x00000001;  // Enable clock for TIM2
 
    TIM2->CR1   = 0x0001;       // CEN(Counter Enable)='1'     < TIM control register 1
    TIM2->SMCR  = 0x0003;       // SMS='011' (Encoder mode 3)  < TIM slave mode control register
    TIM2->CCMR1 = 0x0101;       // CC1S='01' CC2S='01'         < TIM capture/compare mode register 1
    TIM2->CCMR2 = 0x0000;       //                             < TIM capture/compare mode register 2
    TIM2->CCER  = 0x0011;       // CC1P CC2P                   < TIM capture/compare enable register
    TIM2->PSC   = 0x0000;       // Prescaler = (0+1)           < TIM prescaler
    TIM2->ARR   = CPR;          // reload at CPR               < TIM auto-reload register
    TIM2->CNT = 0x0000;         //reset the counter before we use it  
}


//Zero encoder count//
void ZeroEncoder() {
    TIM2->CNT=0 ; //reset timer count to zero
}

int GetCounts(void) {
    int count = TIM2->CNT;  //Read the timer count register
    return count;
    }
    
void pressed() {
    float pwm_float = pwm.read();
    int pwmV = (int)(100*pwm_float);
    if(pwmV == 0){
        pwm.write(0.05);
    } else if (pwmV == 5){
        pwm.write(0.2);
    } else if (pwmV == 20){
        pwm.write(0.75);
    } else if (pwmV == 75){
        pwm.write(0.0);
    } else {
        pwm.write(0.0);
    }
}


int main() {
    
    int endcount, startcount;       // encoder counts
    double time_between_readings;   // between encoder readings
    double velocity;                // radians/time
    double currentSensed = 0;
    clock_t start, end, absoluteStart;
    int ticks;                      // ticks counted on encoder
    a=1; b=0; pwm.write(0);         // this is forward. you can change a and b on a rig if it's turning the wrong way!
    button.fall(&pressed);          //adds pressed callback upon button push
    
    /*  we don't send all the information to matlab all the time. some collection and smoothing is done
        here in order to not overload matlab with input making it slow. And to take a lot of data so we
        can do smoothing quickly on this side */
    double updatePeriod = 0.01;  /* must select carefully. too fast and you don't get enough encoder ticks*/
    double publishFrequency = 0.05; /* seconds. rate to publish to matlab. no need to overload matlab with input*/
    double samplesPerPublish = (int)(publishFrequency/updatePeriod);
    int publishCounter = 1;
    
    double filterRatio = 0.1;           // on the speed data
    double currentFilterRatio = 0.035;  // on the current data
    
    /* the current sensor has some resting value. record and subtract that out */
    float currentSensorOffset = 0; int i; 
    for(i=1;i<301;i++){ currentSensorOffset += currentSense.read(); }
    currentSensorOffset = currentSensorOffset*VREF/300;
    EncoderInitialise();
    fflush(pc);
    
        
    while(1) {
    

        while(1) {                                  // listen for pc go command
            
            green = true;
            if(pc.readable()) {
                char charIn = pc.getc();
                if(charIn == 'g'){ 
                    fflush(pc);
                    absoluteStart = clock();
                    end = clock();
                    ZeroEncoder();
                    velocity = 0;
                    startcount = 0;
                    endcount = 0;
                    currentSensed = 0;
                    pwm.write(pwm_pulley);  
                    break;
                    // note: it's really easy to change this if we want to include
                    // variable speed input from the command in the future.
                    // it's easiest to just pass chars as serial input when the
                    // mbed is listening (I find it's the most reliable)
                    // so an easy way would just be to pass a 1 2 3 ... 9 0
                    // corresponding to some percentage of the max output voltage
                    // as the start value for the run
                    // ask me for previous pulley interface mbed and matlab code
                    // where I have that implimented
                }
            }
        
            wait(0.05);
            
        }  
    
    
        while(1) {                          //start collecting and publishing data
            
            green = false;
            wait(updatePeriod);
            start = end;
            end = clock();        
            time_between_readings = ((double)(end - start)) / CLOCKS_PER_SEC;
            startcount = endcount;
            endcount = GetCounts();       
            ticks = endcount-startcount;
            if(abs(ticks)>CPR/2) /***** for rollover case: *****/
            { ticks = ((ticks<0)-(ticks>0))*(CPR-abs(ticks)); }
            
            velocity = filterRatio*((double)ticks)/CPR*2*PI/time_between_readings + (1-filterRatio)*velocity; /* with filtering*/
            currentSensed = currentFilterRatio*((double)currentSense.read()*VREF-currentSensorOffset) + (1-currentFilterRatio)*currentSensed;
            
            if(pc.readable())                   //pc will send r (reset) when run is finished
            {
                char charIn = pc.getc();
                if(charIn == 'r'){
                    fflush(pc);
                    pwm.write(0.0);
                    break;
                }
            }        
            if(publishCounter == samplesPerPublish) {                           //only publish onece every samplesPerPublish
                printf("%+9.5f,%+8.4f,%+9.4f,%+8.2f\n", currentSensed/currentSensorOutputRatio, pwm.read(), velocity, ((double)(end-absoluteStart)/CLOCKS_PER_SEC));
                publishCounter = 1;  //output formatting very important. running a simple string length checksum on matlab side to discard bad data
            }
            publishCounter++;
            
        }
    
    
    }
    
}