File content as of revision 12:f9f08c7551f4:

///setup code for encoder on pins PA0 and PA1 (A0 and A1)///

// TODO: explain basic flow of run and data collection

#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 power supply
const float Output_Voltage = 2.0;               // Desired output voltage to motor
const float pwm_flywheel = Output_Voltage/PSupply_Voltage;

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;
    double currentSensed = 0;
    clock_t start, end, absoluteStart;
    int ticks;
    a=1; b=0; pwm.write(0);
    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); /*this improves time response of current filter and maintains smoothness*/
    int publishCounter = 1;
    
    /* the filter ratio on the speed data from encoder and on current input */
    double filterRatio = 0.1;
    double currentFilterRatio = 0.035;
    
    /* 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); //clear any previous output
    
        
    while(1) {
    

        
        while(1) {                              //Listen for PC input to start run
            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_flywheel);
                    break; 
                }
            }
            wait(0.05);
        }  
    
    
        
        while(1) {
            
            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' when run is finished
                char charIn = pc.getc();
                if(charIn == 'r'){
                    fflush(pc);
                    pwm.write(0.0);         //kill the motor
                    break;                  //return to listening loop
                }
            }        
            
            if(publishCounter == samplesPerPublish) {           //only publish once 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++;
            
        }
    
    
    }
    
}