2.007 PulleyInterface mbed code. Biomimetics robotics lab. Sangbae Kim. Ben Katz. For use with PulleyInterface.mlapp
main.cpp@1:f97adef77f4b, 2017-02-03 (annotated)
- Committer:
- abraham1
- Date:
- Fri Feb 03 20:09:00 2017 +0000
- Revision:
- 1:f97adef77f4b
- Parent:
- 0:dc5c88c2dd20
- Child:
- 2:a7100c183940
Working on current sensing. 1st order filter on encoding enabled;
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 | 0:dc5c88c2dd20 | 3 | #include "mbed.h" |
abraham1 | 0:dc5c88c2dd20 | 4 | #include "time.h" |
abraham1 | 0:dc5c88c2dd20 | 5 | |
abraham1 | 0:dc5c88c2dd20 | 6 | InterruptIn button(USER_BUTTON); |
abraham1 | 0:dc5c88c2dd20 | 7 | PwmOut pwm(D5);//do not use D3 |
abraham1 | 0:dc5c88c2dd20 | 8 | DigitalOut a(D2); |
abraham1 | 0:dc5c88c2dd20 | 9 | DigitalOut b(D4); |
abraham1 | 0:dc5c88c2dd20 | 10 | |
abraham1 | 0:dc5c88c2dd20 | 11 | int endcount, startcount; |
abraham1 | 0:dc5c88c2dd20 | 12 | double time_between_readings; |
abraham1 | 0:dc5c88c2dd20 | 13 | double velocity; |
abraham1 | 0:dc5c88c2dd20 | 14 | |
abraham1 | 0:dc5c88c2dd20 | 15 | int cpr = 900; // Encoder counts per revolution. Change to match your encoder |
abraham1 | 0:dc5c88c2dd20 | 16 | |
abraham1 | 0:dc5c88c2dd20 | 17 | AnalogIn motorVoltage(A4); //hook up to high voltage motor terminal with capacitor. this doesn't share a common ground though hmm... can I hook up battery ground |
abraham1 | 0:dc5c88c2dd20 | 18 | AnalogIn motorCurrent(A5); //hook up to Vout on current sensor |
abraham1 | 0:dc5c88c2dd20 | 19 | Serial pc(USBTX, USBRX); |
abraham1 | 0:dc5c88c2dd20 | 20 | DigitalOut green(LED2); |
abraham1 | 0:dc5c88c2dd20 | 21 | |
abraham1 | 0:dc5c88c2dd20 | 22 | |
abraham1 | 0:dc5c88c2dd20 | 23 | void EncoderInitialise(void) { |
abraham1 | 0:dc5c88c2dd20 | 24 | // configure GPIO PA0 & PA1 as inputs for Encoder |
abraham1 | 0:dc5c88c2dd20 | 25 | RCC->AHB1ENR |= 0x00000001; // Enable clock for GPIOA |
abraham1 | 0:dc5c88c2dd20 | 26 | |
abraham1 | 0:dc5c88c2dd20 | 27 | 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 | 28 | 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 | 29 | GPIOA->OSPEEDR |= 0x00000011;//|= GPIO_OSPEEDER_OSPEEDR0 | GPIO_OSPEEDER_OSPEEDR1 ; // Low speed /*!< GPIO port output speed register, Address offset: 0x08 */ |
abraham1 | 0:dc5c88c2dd20 | 30 | 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 | 31 | GPIOA->AFR[0] |= 0x00000011 ; // AF01 for PA0 & PA1 /*!< GPIO alternate function registers, Address offset: 0x20-0x24 */ |
abraham1 | 0:dc5c88c2dd20 | 32 | GPIOA->AFR[1] |= 0x00000000 ; // /*!< GPIO alternate function registers, Address offset: 0x20-0x24 */ |
abraham1 | 0:dc5c88c2dd20 | 33 | |
abraham1 | 0:dc5c88c2dd20 | 34 | // configure TIM2 as Encoder input |
abraham1 | 0:dc5c88c2dd20 | 35 | RCC->APB1ENR |= 0x00000001; // Enable clock for TIM2 |
abraham1 | 0:dc5c88c2dd20 | 36 | |
abraham1 | 0:dc5c88c2dd20 | 37 | TIM2->CR1 = 0x0001; // CEN(Counter Enable)='1' < TIM control register 1 |
abraham1 | 0:dc5c88c2dd20 | 38 | TIM2->SMCR = 0x0003; // SMS='011' (Encoder mode 3) < TIM slave mode control register |
abraham1 | 0:dc5c88c2dd20 | 39 | TIM2->CCMR1 = 0x0101; // CC1S='01' CC2S='01' < TIM capture/compare mode register 1 |
abraham1 | 0:dc5c88c2dd20 | 40 | TIM2->CCMR2 = 0x0000; // < TIM capture/compare mode register 2 |
abraham1 | 0:dc5c88c2dd20 | 41 | TIM2->CCER = 0x0011; // CC1P CC2P < TIM capture/compare enable register |
abraham1 | 0:dc5c88c2dd20 | 42 | TIM2->PSC = 0x0000; // Prescaler = (0+1) < TIM prescaler |
abraham1 | 0:dc5c88c2dd20 | 43 | TIM2->ARR = cpr; // reload at cpr < TIM auto-reload register |
abraham1 | 0:dc5c88c2dd20 | 44 | TIM2->CNT = 0x0000; //reset the counter before we use it |
abraham1 | 0:dc5c88c2dd20 | 45 | } |
abraham1 | 0:dc5c88c2dd20 | 46 | |
abraham1 | 0:dc5c88c2dd20 | 47 | |
abraham1 | 0:dc5c88c2dd20 | 48 | //Zero encoder count// |
abraham1 | 0:dc5c88c2dd20 | 49 | void ZeroEncoder() { |
abraham1 | 0:dc5c88c2dd20 | 50 | TIM2->CNT=0 ; //reset timer count to zero |
abraham1 | 0:dc5c88c2dd20 | 51 | } |
abraham1 | 0:dc5c88c2dd20 | 52 | |
abraham1 | 0:dc5c88c2dd20 | 53 | int GetCounts(void) { |
abraham1 | 0:dc5c88c2dd20 | 54 | int count = TIM2->CNT; //Read the timer count register |
abraham1 | 0:dc5c88c2dd20 | 55 | return count; |
abraham1 | 0:dc5c88c2dd20 | 56 | } |
abraham1 | 0:dc5c88c2dd20 | 57 | |
abraham1 | 0:dc5c88c2dd20 | 58 | void pressed() { |
abraham1 | 0:dc5c88c2dd20 | 59 | float pwm_float = pwm.read(); |
abraham1 | 0:dc5c88c2dd20 | 60 | int pwmV = (int)(100*pwm_float); |
abraham1 | 0:dc5c88c2dd20 | 61 | printf("pwm2: %d\n\r",pwmV); |
abraham1 | 0:dc5c88c2dd20 | 62 | if(pwmV == 0){ |
abraham1 | 0:dc5c88c2dd20 | 63 | pwm.write(0.1); |
abraham1 | 0:dc5c88c2dd20 | 64 | } else if (pwmV == 10){ |
abraham1 | 0:dc5c88c2dd20 | 65 | pwm.write(0.2); |
abraham1 | 0:dc5c88c2dd20 | 66 | } else if (pwmV == 20){ |
abraham1 | 0:dc5c88c2dd20 | 67 | pwm.write(0.5); |
abraham1 | 0:dc5c88c2dd20 | 68 | } else if (pwmV == 50){ |
abraham1 | 0:dc5c88c2dd20 | 69 | pwm.write(1.0); |
abraham1 | 0:dc5c88c2dd20 | 70 | } else if (pwmV == 100){ |
abraham1 | 0:dc5c88c2dd20 | 71 | pwm.write(0.0); |
abraham1 | 0:dc5c88c2dd20 | 72 | } |
abraham1 | 0:dc5c88c2dd20 | 73 | } |
abraham1 | 0:dc5c88c2dd20 | 74 | |
abraham1 | 0:dc5c88c2dd20 | 75 | |
abraham1 | 0:dc5c88c2dd20 | 76 | |
abraham1 | 0:dc5c88c2dd20 | 77 | int main() { |
abraham1 | 0:dc5c88c2dd20 | 78 | |
abraham1 | 1:f97adef77f4b | 79 | |
abraham1 | 1:f97adef77f4b | 80 | const double vRef = 3.3; |
abraham1 | 0:dc5c88c2dd20 | 81 | clock_t start; |
abraham1 | 0:dc5c88c2dd20 | 82 | clock_t end = clock(); |
abraham1 | 0:dc5c88c2dd20 | 83 | int ticks; |
abraham1 | 0:dc5c88c2dd20 | 84 | a=1; b=0; pwm.write(0); |
abraham1 | 0:dc5c88c2dd20 | 85 | button.fall(&pressed); |
abraham1 | 0:dc5c88c2dd20 | 86 | double updatePeriod = 0.01; /* must select carefully */ |
abraham1 | 0:dc5c88c2dd20 | 87 | double publishFrequency = 0.1; /* seconds. rate to publish to matlab */ |
abraham1 | 1:f97adef77f4b | 88 | double samplesPerPublish = (int)(publishFrequency/updatePeriod); /*this improves time response of filter and maintains smoothness*/ |
abraham1 | 0:dc5c88c2dd20 | 89 | int publishCounter = 1; |
abraham1 | 1:f97adef77f4b | 90 | double filterRatio = 0.05; |
abraham1 | 1:f97adef77f4b | 91 | float currentSensorOffset = 0; int i; |
abraham1 | 1:f97adef77f4b | 92 | for(i=1;i<101;i++){ currentSensorOffset += motorCurrent.read(); } |
abraham1 | 1:f97adef77f4b | 93 | currentSensorOffset = currentSensorOffset*vRef/100; |
abraham1 | 1:f97adef77f4b | 94 | |
abraham1 | 1:f97adef77f4b | 95 | EncoderInitialise(); |
abraham1 | 1:f97adef77f4b | 96 | fflush(pc); |
abraham1 | 1:f97adef77f4b | 97 | |
abraham1 | 1:f97adef77f4b | 98 | |
abraham1 | 0:dc5c88c2dd20 | 99 | while(1) { |
abraham1 | 0:dc5c88c2dd20 | 100 | |
abraham1 | 0:dc5c88c2dd20 | 101 | wait(updatePeriod); |
abraham1 | 0:dc5c88c2dd20 | 102 | start = end; |
abraham1 | 0:dc5c88c2dd20 | 103 | end = clock(); |
abraham1 | 0:dc5c88c2dd20 | 104 | time_between_readings = ((double)(end - start)) / CLOCKS_PER_SEC; |
abraham1 | 0:dc5c88c2dd20 | 105 | startcount = endcount; |
abraham1 | 0:dc5c88c2dd20 | 106 | endcount = GetCounts(); |
abraham1 | 0:dc5c88c2dd20 | 107 | ticks = endcount-startcount; |
abraham1 | 0:dc5c88c2dd20 | 108 | if(abs(ticks)>cpr/2) /***** for rollover case: *****/ |
abraham1 | 0:dc5c88c2dd20 | 109 | { ticks = ((ticks<0)-(ticks>0))*(cpr-abs(ticks)); } |
abraham1 | 0:dc5c88c2dd20 | 110 | velocity = filterRatio*((double)ticks)/cpr/time_between_readings + (1-filterRatio)*velocity; /* with filtering*/ |
abraham1 | 0:dc5c88c2dd20 | 111 | if(pc.readable()) |
abraham1 | 0:dc5c88c2dd20 | 112 | { |
abraham1 | 0:dc5c88c2dd20 | 113 | char abrahamsCommand = pc.getc(); |
abraham1 | 0:dc5c88c2dd20 | 114 | if(abrahamsCommand == '1'){ green = 1; } |
abraham1 | 0:dc5c88c2dd20 | 115 | if(abrahamsCommand == '0'){ green = 0; } |
abraham1 | 0:dc5c88c2dd20 | 116 | } |
abraham1 | 1:f97adef77f4b | 117 | if(publishCounter == samplesPerPublish){ |
abraham1 | 1:f97adef77f4b | 118 | printf("%f,%f,%f\n", motorCurrent.read()*vRef-(float)currentSensorOffset, motorVoltage.read(), velocity); |
abraham1 | 1:f97adef77f4b | 119 | publishCounter = 1; |
abraham1 | 1:f97adef77f4b | 120 | } |
abraham1 | 1:f97adef77f4b | 121 | publishCounter++; |
abraham1 | 0:dc5c88c2dd20 | 122 | |
abraham1 | 0:dc5c88c2dd20 | 123 | } |
abraham1 | 0:dc5c88c2dd20 | 124 | |
abraham1 | 0:dc5c88c2dd20 | 125 | |
abraham1 | 0:dc5c88c2dd20 | 126 | } |
abraham1 | 0:dc5c88c2dd20 | 127 | |
abraham1 | 0:dc5c88c2dd20 | 128 | |
abraham1 | 0:dc5c88c2dd20 | 129 | |
abraham1 | 0:dc5c88c2dd20 | 130 | |
abraham1 | 0:dc5c88c2dd20 | 131 | |
abraham1 | 0:dc5c88c2dd20 | 132 | |
abraham1 | 0:dc5c88c2dd20 | 133 | |
abraham1 | 0:dc5c88c2dd20 | 134 |