main.cpp

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