For flywheel rig. Interfaces with matlab flywheel interface. 2.007 MIT Biomimetics Robotics Lab. Sangbae Kim. Ben Katz.
main.cpp@12:f9f08c7551f4, 2017-04-14 (annotated)
- Committer:
- abraham1
- Date:
- Fri Apr 14 18:06:13 2017 +0000
- Revision:
- 12:f9f08c7551f4
- Parent:
- 11:ad41446b0edb
Commented code;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
abraham1 | 0:dc5c88c2dd20 | 1 | ///setup code for encoder on pins PA0 and PA1 (A0 and A1)/// |
abraham1 | 0:dc5c88c2dd20 | 2 | |
abraham1 | 12:f9f08c7551f4 | 3 | // TODO: explain basic flow of run and data collection |
abraham1 | 12:f9f08c7551f4 | 4 | |
abraham1 | 0:dc5c88c2dd20 | 5 | #include "mbed.h" |
abraham1 | 0:dc5c88c2dd20 | 6 | #include "time.h" |
abraham1 | 3:df56bf381572 | 7 | #include "stdio.h" |
abraham1 | 3:df56bf381572 | 8 | #include "ctype.h" |
abraham1 | 0:dc5c88c2dd20 | 9 | |
abraham1 | 4:f8a45966e63b | 10 | #define PI 3.14159265358979323846 |
abraham1 | 4:f8a45966e63b | 11 | |
abraham1 | 12:f9f08c7551f4 | 12 | //Just for basic debugging |
abraham1 | 12:f9f08c7551f4 | 13 | //User button controls motor speed |
abraham1 | 12:f9f08c7551f4 | 14 | //Green LED should turn on while listening to pc for input before starting run |
abraham1 | 0:dc5c88c2dd20 | 15 | InterruptIn button(USER_BUTTON); |
abraham1 | 3:df56bf381572 | 16 | DigitalOut green(LED2); |
abraham1 | 0:dc5c88c2dd20 | 17 | |
abraham1 | 12:f9f08c7551f4 | 18 | // Pololu VNH5019 Motor Driver Carrier. https://www.pololu.com/product/1451 |
abraham1 | 12:f9f08c7551f4 | 19 | PwmOut pwm(D5); //pwm input on motor controller. do not use D3 |
abraham1 | 12:f9f08c7551f4 | 20 | DigitalOut a(D2); //IN_A input on motor controller |
abraham1 | 12:f9f08c7551f4 | 21 | DigitalOut b(D4); //IN_B input on motor controller |
abraham1 | 12:f9f08c7551f4 | 22 | |
abraham1 | 12:f9f08c7551f4 | 23 | //Hook up to Vout on current sensor |
abraham1 | 12:f9f08c7551f4 | 24 | //SparkFun Hall-Effect Current Sensor Breakout - ACS712 |
abraham1 | 12:f9f08c7551f4 | 25 | //https://www.sparkfun.com/products/8882 |
abraham1 | 12:f9f08c7551f4 | 26 | AnalogIn currentSense(A5); |
abraham1 | 12:f9f08c7551f4 | 27 | |
abraham1 | 12:f9f08c7551f4 | 28 | //For communication with pc through matlab |
abraham1 | 12:f9f08c7551f4 | 29 | //Make sure baud rates are equal |
abraham1 | 12:f9f08c7551f4 | 30 | Serial pc(USBTX, USBRX, 115200); |
abraham1 | 12:f9f08c7551f4 | 31 | |
abraham1 | 12:f9f08c7551f4 | 32 | const int CPR = 900*4; // Encoder counts per revolution (900). Change to match your encoder. x4 for quadrature |
abraham1 | 12:f9f08c7551f4 | 33 | const double VREF = 3; // Microcontroller reference voltage |
abraham1 | 12:f9f08c7551f4 | 34 | const float currentSensorOutputRatio = 0.185; // Volts/Amp specified by current sensor. Divide Voltage by cSenseOutput to get current |
abraham1 | 12:f9f08c7551f4 | 35 | const float PSupply_Voltage = 12.0; // Voltage input from power supply |
abraham1 | 12:f9f08c7551f4 | 36 | const float Output_Voltage = 2.0; // Desired output voltage to motor |
abraham1 | 11:ad41446b0edb | 37 | const float pwm_flywheel = Output_Voltage/PSupply_Voltage; |
abraham1 | 0:dc5c88c2dd20 | 38 | |
abraham1 | 0:dc5c88c2dd20 | 39 | void EncoderInitialise(void) { |
abraham1 | 0:dc5c88c2dd20 | 40 | // configure GPIO PA0 & PA1 as inputs for Encoder |
abraham1 | 0:dc5c88c2dd20 | 41 | RCC->AHB1ENR |= 0x00000001; // Enable clock for GPIOA |
abraham1 | 0:dc5c88c2dd20 | 42 | |
abraham1 | 0:dc5c88c2dd20 | 43 | GPIOA->MODER |= GPIO_MODER_MODER0_1 | GPIO_MODER_MODER1_1 ; //PA0 & PA1 as Alternate Function /*!< GPIO port mode register, Address offset: 0x00 */ |
abraham1 | 0:dc5c88c2dd20 | 44 | GPIOA->OTYPER |= GPIO_OTYPER_OT_0 | GPIO_OTYPER_OT_1 ; //PA0 & PA1 as Inputs /*!< GPIO port output type register, Address offset: 0x04 */ |
abraham1 | 0:dc5c88c2dd20 | 45 | GPIOA->OSPEEDR |= 0x00000011;//|= GPIO_OSPEEDER_OSPEEDR0 | GPIO_OSPEEDER_OSPEEDR1 ; // Low speed /*!< GPIO port output speed register, Address offset: 0x08 */ |
abraham1 | 0:dc5c88c2dd20 | 46 | GPIOA->PUPDR |= GPIO_PUPDR_PUPDR0_1 | GPIO_PUPDR_PUPDR1_1 ; // Pull Down /*!< GPIO port pull-up/pull-down register, Address offset: 0x0C */ |
abraham1 | 0:dc5c88c2dd20 | 47 | GPIOA->AFR[0] |= 0x00000011 ; // AF01 for PA0 & PA1 /*!< GPIO alternate function registers, Address offset: 0x20-0x24 */ |
abraham1 | 0:dc5c88c2dd20 | 48 | GPIOA->AFR[1] |= 0x00000000 ; // /*!< GPIO alternate function registers, Address offset: 0x20-0x24 */ |
abraham1 | 0:dc5c88c2dd20 | 49 | |
abraham1 | 0:dc5c88c2dd20 | 50 | // configure TIM2 as Encoder input |
abraham1 | 0:dc5c88c2dd20 | 51 | RCC->APB1ENR |= 0x00000001; // Enable clock for TIM2 |
abraham1 | 0:dc5c88c2dd20 | 52 | |
abraham1 | 0:dc5c88c2dd20 | 53 | TIM2->CR1 = 0x0001; // CEN(Counter Enable)='1' < TIM control register 1 |
abraham1 | 0:dc5c88c2dd20 | 54 | TIM2->SMCR = 0x0003; // SMS='011' (Encoder mode 3) < TIM slave mode control register |
abraham1 | 0:dc5c88c2dd20 | 55 | TIM2->CCMR1 = 0x0101; // CC1S='01' CC2S='01' < TIM capture/compare mode register 1 |
abraham1 | 0:dc5c88c2dd20 | 56 | TIM2->CCMR2 = 0x0000; // < TIM capture/compare mode register 2 |
abraham1 | 0:dc5c88c2dd20 | 57 | TIM2->CCER = 0x0011; // CC1P CC2P < TIM capture/compare enable register |
abraham1 | 0:dc5c88c2dd20 | 58 | TIM2->PSC = 0x0000; // Prescaler = (0+1) < TIM prescaler |
abraham1 | 4:f8a45966e63b | 59 | TIM2->ARR = CPR; // reload at CPR < TIM auto-reload register |
abraham1 | 0:dc5c88c2dd20 | 60 | TIM2->CNT = 0x0000; //reset the counter before we use it |
abraham1 | 0:dc5c88c2dd20 | 61 | } |
abraham1 | 0:dc5c88c2dd20 | 62 | |
abraham1 | 0:dc5c88c2dd20 | 63 | |
abraham1 | 0:dc5c88c2dd20 | 64 | //Zero encoder count// |
abraham1 | 0:dc5c88c2dd20 | 65 | void ZeroEncoder() { |
abraham1 | 0:dc5c88c2dd20 | 66 | TIM2->CNT=0 ; //reset timer count to zero |
abraham1 | 0:dc5c88c2dd20 | 67 | } |
abraham1 | 0:dc5c88c2dd20 | 68 | |
abraham1 | 0:dc5c88c2dd20 | 69 | int GetCounts(void) { |
abraham1 | 0:dc5c88c2dd20 | 70 | int count = TIM2->CNT; //Read the timer count register |
abraham1 | 0:dc5c88c2dd20 | 71 | return count; |
abraham1 | 0:dc5c88c2dd20 | 72 | } |
abraham1 | 0:dc5c88c2dd20 | 73 | |
abraham1 | 0:dc5c88c2dd20 | 74 | void pressed() { |
abraham1 | 0:dc5c88c2dd20 | 75 | float pwm_float = pwm.read(); |
abraham1 | 0:dc5c88c2dd20 | 76 | int pwmV = (int)(100*pwm_float); |
abraham1 | 0:dc5c88c2dd20 | 77 | if(pwmV == 0){ |
abraham1 | 3:df56bf381572 | 78 | pwm.write(0.05); |
abraham1 | 3:df56bf381572 | 79 | } else if (pwmV == 5){ |
abraham1 | 0:dc5c88c2dd20 | 80 | pwm.write(0.2); |
abraham1 | 0:dc5c88c2dd20 | 81 | } else if (pwmV == 20){ |
abraham1 | 3:df56bf381572 | 82 | pwm.write(0.75); |
abraham1 | 3:df56bf381572 | 83 | } else if (pwmV == 75){ |
abraham1 | 0:dc5c88c2dd20 | 84 | pwm.write(0.0); |
abraham1 | 3:df56bf381572 | 85 | } else { |
abraham1 | 3:df56bf381572 | 86 | pwm.write(0.0); |
abraham1 | 3:df56bf381572 | 87 | } |
abraham1 | 0:dc5c88c2dd20 | 88 | } |
abraham1 | 0:dc5c88c2dd20 | 89 | |
abraham1 | 0:dc5c88c2dd20 | 90 | |
abraham1 | 0:dc5c88c2dd20 | 91 | int main() { |
abraham1 | 0:dc5c88c2dd20 | 92 | |
abraham1 | 12:f9f08c7551f4 | 93 | int endcount, startcount; //encoder counts |
abraham1 | 12:f9f08c7551f4 | 94 | double time_between_readings; //between encoder readings |
abraham1 | 3:df56bf381572 | 95 | double velocity; |
abraham1 | 3:df56bf381572 | 96 | double currentSensed = 0; |
abraham1 | 7:1726c40ad774 | 97 | clock_t start, end, absoluteStart; |
abraham1 | 0:dc5c88c2dd20 | 98 | int ticks; |
abraham1 | 0:dc5c88c2dd20 | 99 | a=1; b=0; pwm.write(0); |
abraham1 | 12:f9f08c7551f4 | 100 | button.fall(&pressed); //adds pressed callback upon button push |
abraham1 | 12:f9f08c7551f4 | 101 | |
abraham1 | 12:f9f08c7551f4 | 102 | /* we don't send all the information to matlab all the time. some collection and smoothing is done |
abraham1 | 12:f9f08c7551f4 | 103 | here in order to not overload matlab with input making it slow. And to take a lot of data so we |
abraham1 | 12:f9f08c7551f4 | 104 | can do smoothing quickly on this side */ |
abraham1 | 12:f9f08c7551f4 | 105 | double updatePeriod = 0.01; /* must select carefully. too fast and you don't get enough encoder ticks*/ |
abraham1 | 12:f9f08c7551f4 | 106 | double publishFrequency = 0.05; /* seconds. rate to publish to matlab. no need to overload matlab with input*/ |
abraham1 | 12:f9f08c7551f4 | 107 | double samplesPerPublish = (int)(publishFrequency/updatePeriod); /*this improves time response of current filter and maintains smoothness*/ |
abraham1 | 0:dc5c88c2dd20 | 108 | int publishCounter = 1; |
abraham1 | 12:f9f08c7551f4 | 109 | |
abraham1 | 12:f9f08c7551f4 | 110 | /* the filter ratio on the speed data from encoder and on current input */ |
abraham1 | 10:ae139f40acc0 | 111 | double filterRatio = 0.1; |
abraham1 | 8:6ae3c3cd7f55 | 112 | double currentFilterRatio = 0.035; |
abraham1 | 12:f9f08c7551f4 | 113 | |
abraham1 | 12:f9f08c7551f4 | 114 | /* the current sensor has some resting value. record and subtract that out */ |
abraham1 | 1:f97adef77f4b | 115 | float currentSensorOffset = 0; int i; |
abraham1 | 4:f8a45966e63b | 116 | for(i=1;i<301;i++){ currentSensorOffset += currentSense.read(); } |
abraham1 | 4:f8a45966e63b | 117 | currentSensorOffset = currentSensorOffset*VREF/300; |
abraham1 | 12:f9f08c7551f4 | 118 | |
abraham1 | 1:f97adef77f4b | 119 | EncoderInitialise(); |
abraham1 | 12:f9f08c7551f4 | 120 | fflush(pc); //clear any previous output |
abraham1 | 1:f97adef77f4b | 121 | |
abraham1 | 11:ad41446b0edb | 122 | |
abraham1 | 0:dc5c88c2dd20 | 123 | while(1) { |
abraham1 | 5:f4c237d0bb32 | 124 | |
abraham1 | 10:ae139f40acc0 | 125 | |
abraham1 | 12:f9f08c7551f4 | 126 | |
abraham1 | 12:f9f08c7551f4 | 127 | while(1) { //Listen for PC input to start run |
abraham1 | 5:f4c237d0bb32 | 128 | green = true; |
abraham1 | 11:ad41446b0edb | 129 | if(pc.readable()) { |
abraham1 | 5:f4c237d0bb32 | 130 | char charIn = pc.getc(); |
abraham1 | 6:73e417b1c521 | 131 | if(charIn == 'g'){ |
abraham1 | 11:ad41446b0edb | 132 | fflush(pc); |
abraham1 | 7:1726c40ad774 | 133 | absoluteStart = clock(); |
abraham1 | 11:ad41446b0edb | 134 | end = clock(); |
abraham1 | 6:73e417b1c521 | 135 | ZeroEncoder(); |
abraham1 | 6:73e417b1c521 | 136 | velocity = 0; |
abraham1 | 6:73e417b1c521 | 137 | startcount = 0; |
abraham1 | 6:73e417b1c521 | 138 | endcount = 0; |
abraham1 | 6:73e417b1c521 | 139 | currentSensed = 0; |
abraham1 | 11:ad41446b0edb | 140 | pwm.write(pwm_flywheel); |
abraham1 | 6:73e417b1c521 | 141 | break; |
abraham1 | 6:73e417b1c521 | 142 | } |
abraham1 | 3:df56bf381572 | 143 | } |
abraham1 | 5:f4c237d0bb32 | 144 | wait(0.05); |
abraham1 | 5:f4c237d0bb32 | 145 | } |
abraham1 | 5:f4c237d0bb32 | 146 | |
abraham1 | 5:f4c237d0bb32 | 147 | |
abraham1 | 12:f9f08c7551f4 | 148 | |
abraham1 | 5:f4c237d0bb32 | 149 | while(1) { |
abraham1 | 7:1726c40ad774 | 150 | |
abraham1 | 5:f4c237d0bb32 | 151 | green = false; |
abraham1 | 5:f4c237d0bb32 | 152 | wait(updatePeriod); |
abraham1 | 5:f4c237d0bb32 | 153 | start = end; |
abraham1 | 5:f4c237d0bb32 | 154 | end = clock(); |
abraham1 | 5:f4c237d0bb32 | 155 | time_between_readings = ((double)(end - start)) / CLOCKS_PER_SEC; |
abraham1 | 5:f4c237d0bb32 | 156 | startcount = endcount; |
abraham1 | 5:f4c237d0bb32 | 157 | endcount = GetCounts(); |
abraham1 | 5:f4c237d0bb32 | 158 | ticks = endcount-startcount; |
abraham1 | 5:f4c237d0bb32 | 159 | if(abs(ticks)>CPR/2) /***** for rollover case: *****/ |
abraham1 | 5:f4c237d0bb32 | 160 | { ticks = ((ticks<0)-(ticks>0))*(CPR-abs(ticks)); } |
abraham1 | 12:f9f08c7551f4 | 161 | |
abraham1 | 5:f4c237d0bb32 | 162 | velocity = filterRatio*((double)ticks)/CPR*2*PI/time_between_readings + (1-filterRatio)*velocity; /* with filtering*/ |
abraham1 | 5:f4c237d0bb32 | 163 | currentSensed = currentFilterRatio*((double)currentSense.read()*VREF-currentSensorOffset) + (1-currentFilterRatio)*currentSensed; |
abraham1 | 12:f9f08c7551f4 | 164 | |
abraham1 | 12:f9f08c7551f4 | 165 | if(pc.readable()) { //PC will send 'r' when run is finished |
abraham1 | 5:f4c237d0bb32 | 166 | char charIn = pc.getc(); |
abraham1 | 5:f4c237d0bb32 | 167 | if(charIn == 'r'){ |
abraham1 | 11:ad41446b0edb | 168 | fflush(pc); |
abraham1 | 12:f9f08c7551f4 | 169 | pwm.write(0.0); //kill the motor |
abraham1 | 12:f9f08c7551f4 | 170 | break; //return to listening loop |
abraham1 | 5:f4c237d0bb32 | 171 | } |
abraham1 | 5:f4c237d0bb32 | 172 | } |
abraham1 | 12:f9f08c7551f4 | 173 | |
abraham1 | 12:f9f08c7551f4 | 174 | if(publishCounter == samplesPerPublish) { //only publish once every samplesPerPublish |
abraham1 | 11:ad41446b0edb | 175 | printf("%+9.5f,%+8.4f,%+9.4f,%+8.2f\n", currentSensed/currentSensorOutputRatio, pwm.read(), -velocity, ((double)(end-absoluteStart)/CLOCKS_PER_SEC)); |
abraham1 | 12:f9f08c7551f4 | 176 | publishCounter = 1; //output formatting very important. running a simple string length checksum on matlab side to discard bad data |
abraham1 | 7:1726c40ad774 | 177 | } |
abraham1 | 5:f4c237d0bb32 | 178 | publishCounter++; |
abraham1 | 5:f4c237d0bb32 | 179 | |
abraham1 | 5:f4c237d0bb32 | 180 | } |
abraham1 | 5:f4c237d0bb32 | 181 | |
abraham1 | 7:1726c40ad774 | 182 | |
abraham1 | 0:dc5c88c2dd20 | 183 | } |
abraham1 | 0:dc5c88c2dd20 | 184 | |
abraham1 | 0:dc5c88c2dd20 | 185 | } |
abraham1 | 0:dc5c88c2dd20 | 186 | |
abraham1 | 0:dc5c88c2dd20 | 187 | |
abraham1 | 0:dc5c88c2dd20 | 188 | |
abraham1 | 0:dc5c88c2dd20 | 189 | |
abraham1 | 0:dc5c88c2dd20 | 190 | |
abraham1 | 0:dc5c88c2dd20 | 191 | |
abraham1 | 0:dc5c88c2dd20 | 192 | |
abraham1 | 0:dc5c88c2dd20 | 193 |