For flywheel rig. Interfaces with matlab flywheel interface. 2.007 MIT Biomimetics Robotics Lab. Sangbae Kim. Ben Katz.
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
Generated on Sun Jul 24 2022 13:45:56 by 1.7.2