main.cpp

00001 ///setup code for encoder on pins PA0 and PA1 (A0 and A1)///
00002 
00003 // TODO: explain basic flow of run and data collection
00004 
00005 #include "mbed.h"
00006 #include "time.h"
00007 #include "stdio.h"
00008 #include "ctype.h"
00009 
00010 #define PI 3.14159265358979323846
00011 
00012 //Just for basic debugging
00013 //User button controls motor speed
00014 //Green LED should turn on while listening to pc for input before starting run
00015 InterruptIn button(USER_BUTTON);
00016 DigitalOut green(LED2);
00017 
00018 // Pololu VNH5019 Motor Driver Carrier. https://www.pololu.com/product/1451
00019 PwmOut pwm(D5);     //pwm input on motor controller. do not use D3
00020 DigitalOut a(D2);   //IN_A input on motor controller
00021 DigitalOut b(D4);   //IN_B input on motor controller
00022 
00023 //Hook up to Vout on current sensor
00024 //SparkFun Hall-Effect Current Sensor Breakout - ACS712
00025 //https://www.sparkfun.com/products/8882
00026 AnalogIn currentSense(A5);
00027 
00028 //For communication with pc through matlab
00029 //Make sure baud rates are equal
00030 Serial pc(USBTX, USBRX, 115200);
00031 
00032 const int CPR = 900*4;  // Encoder counts per revolution (900).  Change to match your encoder. x4 for quadrature
00033 const double VREF = 3;  // Microcontroller reference voltage
00034 const float currentSensorOutputRatio = 0.185;   // Volts/Amp specified by current sensor. Divide Voltage by cSenseOutput to get current
00035 const float PSupply_Voltage = 12.0;             // Voltage input from power supply
00036 const float Output_Voltage = 2.0;               // Desired output voltage to motor
00037 const float pwm_flywheel = Output_Voltage/PSupply_Voltage;
00038 
00039 void EncoderInitialise(void) {
00040     // configure GPIO PA0 & PA1 as inputs for Encoder
00041     RCC->AHB1ENR |= 0x00000001;  // Enable clock for GPIOA
00042  
00043     GPIOA->MODER   |= GPIO_MODER_MODER0_1 | GPIO_MODER_MODER1_1 ;           //PA0 & PA1 as Alternate Function   /*!< GPIO port mode register,               Address offset: 0x00      */
00044     GPIOA->OTYPER  |= GPIO_OTYPER_OT_0 | GPIO_OTYPER_OT_1 ;                 //PA0 & PA1 as Inputs               /*!< GPIO port output type register,        Address offset: 0x04      */
00045     GPIOA->OSPEEDR |= 0x00000011;//|= GPIO_OSPEEDER_OSPEEDR0 | GPIO_OSPEEDER_OSPEEDR1 ;     // Low speed                        /*!< GPIO port output speed register,       Address offset: 0x08      */
00046     GPIOA->PUPDR   |= GPIO_PUPDR_PUPDR0_1 | GPIO_PUPDR_PUPDR1_1 ;           // Pull Down                        /*!< GPIO port pull-up/pull-down register,  Address offset: 0x0C      */
00047     GPIOA->AFR[0]  |= 0x00000011 ;                                          //  AF01 for PA0 & PA1              /*!< GPIO alternate function registers,     Address offset: 0x20-0x24 */
00048     GPIOA->AFR[1]  |= 0x00000000 ;                                          //                                  /*!< GPIO alternate function registers,     Address offset: 0x20-0x24 */
00049    
00050     // configure TIM2 as Encoder input
00051     RCC->APB1ENR |= 0x00000001;  // Enable clock for TIM2
00052  
00053     TIM2->CR1   = 0x0001;       // CEN(Counter Enable)='1'     < TIM control register 1
00054     TIM2->SMCR  = 0x0003;       // SMS='011' (Encoder mode 3)  < TIM slave mode control register
00055     TIM2->CCMR1 = 0x0101;       // CC1S='01' CC2S='01'         < TIM capture/compare mode register 1
00056     TIM2->CCMR2 = 0x0000;       //                             < TIM capture/compare mode register 2
00057     TIM2->CCER  = 0x0011;       // CC1P CC2P                   < TIM capture/compare enable register
00058     TIM2->PSC   = 0x0000;       // Prescaler = (0+1)           < TIM prescaler
00059     TIM2->ARR   = CPR;          // reload at CPR               < TIM auto-reload register
00060     TIM2->CNT = 0x0000;         //reset the counter before we use it  
00061 }
00062 
00063 
00064 //Zero encoder count//
00065 void ZeroEncoder() {
00066     TIM2->CNT=0 ; //reset timer count to zero
00067 }
00068 
00069 int GetCounts(void) {
00070     int count = TIM2->CNT;  //Read the timer count register
00071     return count;
00072     }
00073     
00074 void pressed() {
00075     float pwm_float = pwm.read();
00076     int pwmV = (int)(100*pwm_float);
00077     if(pwmV == 0){
00078         pwm.write(0.05);
00079     } else if (pwmV == 5){
00080         pwm.write(0.2);
00081     } else if (pwmV == 20){
00082         pwm.write(0.75);
00083     } else if (pwmV == 75){
00084         pwm.write(0.0);
00085     } else {
00086         pwm.write(0.0);
00087     }
00088 }
00089 
00090 
00091 int main() {
00092     
00093     int endcount, startcount; //encoder counts
00094     double time_between_readings; //between encoder readings
00095     double velocity;
00096     double currentSensed = 0;
00097     clock_t start, end, absoluteStart;
00098     int ticks;
00099     a=1; b=0; pwm.write(0);
00100     button.fall(&pressed);  //adds pressed callback upon button push
00101     
00102     /*  we don't send all the information to matlab all the time. some collection and smoothing is done
00103         here in order to not overload matlab with input making it slow. And to take a lot of data so we
00104         can do smoothing quickly on this side */
00105     double updatePeriod = 0.01; /* must select carefully. too fast and you don't get enough encoder ticks*/
00106     double publishFrequency = 0.05; /* seconds. rate to publish to matlab. no need to overload matlab with input*/
00107     double samplesPerPublish = (int)(publishFrequency/updatePeriod); /*this improves time response of current filter and maintains smoothness*/
00108     int publishCounter = 1;
00109     
00110     /* the filter ratio on the speed data from encoder and on current input */
00111     double filterRatio = 0.1;
00112     double currentFilterRatio = 0.035;
00113     
00114     /* the current sensor has some resting value. record and subtract that out */
00115     float currentSensorOffset = 0; int i; 
00116     for(i=1;i<301;i++){ currentSensorOffset += currentSense.read(); }
00117     currentSensorOffset = currentSensorOffset*VREF/300;
00118     
00119     EncoderInitialise();
00120     fflush(pc); //clear any previous output
00121     
00122         
00123     while(1) {
00124     
00125 
00126         
00127         while(1) {                              //Listen for PC input to start run
00128             green = true;
00129             if(pc.readable()) {
00130                 char charIn = pc.getc();
00131                 if(charIn == 'g'){ 
00132                     fflush(pc);
00133                     absoluteStart = clock();
00134                     end = clock();
00135                     ZeroEncoder();
00136                     velocity = 0;
00137                     startcount = 0;
00138                     endcount = 0;
00139                     currentSensed = 0;
00140                     pwm.write(pwm_flywheel);
00141                     break; 
00142                 }
00143             }
00144             wait(0.05);
00145         }  
00146     
00147     
00148         
00149         while(1) {
00150             
00151             green = false;
00152             wait(updatePeriod);
00153             start = end;
00154             end = clock();        
00155             time_between_readings = ((double)(end - start)) / CLOCKS_PER_SEC;
00156             startcount = endcount;
00157             endcount = GetCounts();       
00158             ticks = endcount-startcount;
00159             if(abs(ticks)>CPR/2) /***** for rollover case: *****/
00160             { ticks = ((ticks<0)-(ticks>0))*(CPR-abs(ticks)); }
00161             
00162             velocity = filterRatio*((double)ticks)/CPR*2*PI/time_between_readings + (1-filterRatio)*velocity; /* with filtering*/
00163             currentSensed = currentFilterRatio*((double)currentSense.read()*VREF-currentSensorOffset) + (1-currentFilterRatio)*currentSensed;
00164             
00165             if(pc.readable()) {             //PC will send 'r' when run is finished
00166                 char charIn = pc.getc();
00167                 if(charIn == 'r'){
00168                     fflush(pc);
00169                     pwm.write(0.0);         //kill the motor
00170                     break;                  //return to listening loop
00171                 }
00172             }        
00173             
00174             if(publishCounter == samplesPerPublish) {           //only publish once every samplesPerPublish
00175                 printf("%+9.5f,%+8.4f,%+9.4f,%+8.2f\n", currentSensed/currentSensorOutputRatio, pwm.read(), -velocity, ((double)(end-absoluteStart)/CLOCKS_PER_SEC));
00176                 publishCounter = 1;  //output formatting very important. running a simple string length checksum on matlab side to discard bad data
00177             }
00178             publishCounter++;
00179             
00180         }
00181     
00182     
00183     }
00184     
00185 }
00186 
00187 
00188 
00189 
00190 
00191 
00192 
00193