For flywheel rig. Interfaces with matlab flywheel interface. 2.007 MIT Biomimetics Robotics Lab. Sangbae Kim. Ben Katz.
Diff: main.cpp
- Revision:
- 0:dc5c88c2dd20
- Child:
- 1:f97adef77f4b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Feb 03 18:18:22 2017 +0000 @@ -0,0 +1,130 @@ +///setup code for encoder on pins PA0 and PA1 (A0 and A1)/// + +#include "mbed.h" +#include "time.h" + +InterruptIn button(USER_BUTTON); +PwmOut pwm(D5);//do not use D3 +DigitalOut a(D2); +DigitalOut b(D4); + +int endcount, startcount; +double time_between_readings; +double velocity; + +int cpr = 900; // Encoder counts per revolution. Change to match your encoder + +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 +AnalogIn motorCurrent(A5); //hook up to Vout on current sensor +Serial pc(USBTX, USBRX); +DigitalOut green(LED2); + + +void EncoderInitialise(void) { + // configure GPIO PA0 & PA1 as inputs for Encoder + RCC->AHB1ENR |= 0x00000001; // Enable clock for GPIOA + + GPIOA->MODER |= GPIO_MODER_MODER0_1 | GPIO_MODER_MODER1_1 ; //PA0 & PA1 as Alternate Function /*!< GPIO port mode register, Address offset: 0x00 */ + GPIOA->OTYPER |= GPIO_OTYPER_OT_0 | GPIO_OTYPER_OT_1 ; //PA0 & PA1 as Inputs /*!< GPIO port output type register, Address offset: 0x04 */ + GPIOA->OSPEEDR |= 0x00000011;//|= GPIO_OSPEEDER_OSPEEDR0 | GPIO_OSPEEDER_OSPEEDR1 ; // Low speed /*!< GPIO port output speed register, Address offset: 0x08 */ + GPIOA->PUPDR |= GPIO_PUPDR_PUPDR0_1 | GPIO_PUPDR_PUPDR1_1 ; // Pull Down /*!< GPIO port pull-up/pull-down register, Address offset: 0x0C */ + GPIOA->AFR[0] |= 0x00000011 ; // AF01 for PA0 & PA1 /*!< GPIO alternate function registers, Address offset: 0x20-0x24 */ + GPIOA->AFR[1] |= 0x00000000 ; // /*!< GPIO alternate function registers, Address offset: 0x20-0x24 */ + + // configure TIM2 as Encoder input + RCC->APB1ENR |= 0x00000001; // Enable clock for TIM2 + + TIM2->CR1 = 0x0001; // CEN(Counter Enable)='1' < TIM control register 1 + TIM2->SMCR = 0x0003; // SMS='011' (Encoder mode 3) < TIM slave mode control register + TIM2->CCMR1 = 0x0101; // CC1S='01' CC2S='01' < TIM capture/compare mode register 1 + TIM2->CCMR2 = 0x0000; // < TIM capture/compare mode register 2 + TIM2->CCER = 0x0011; // CC1P CC2P < TIM capture/compare enable register + TIM2->PSC = 0x0000; // Prescaler = (0+1) < TIM prescaler + TIM2->ARR = cpr; // reload at cpr < TIM auto-reload register + TIM2->CNT = 0x0000; //reset the counter before we use it +} + + +//Zero encoder count// +void ZeroEncoder() { + TIM2->CNT=0 ; //reset timer count to zero +} + +int GetCounts(void) { + int count = TIM2->CNT; //Read the timer count register + return count; + } + +void pressed() { + float pwm_float = pwm.read(); + int pwmV = (int)(100*pwm_float); + printf("pwm2: %d\n\r",pwmV); + if(pwmV == 0){ + pwm.write(0.1); + } else if (pwmV == 10){ + pwm.write(0.2); + } else if (pwmV == 20){ + pwm.write(0.5); + } else if (pwmV == 50){ + pwm.write(1.0); + } else if (pwmV == 100){ + pwm.write(0.0); + } +} + + + +int main() { + + EncoderInitialise(); + clock_t start; + clock_t end = clock(); + int ticks; + a=1; b=0; pwm.write(0); + button.fall(&pressed); + double updatePeriod = 0.01; /* must select carefully */ + double publishFrequency = 0.1; /* seconds. rate to publish to matlab */ + double samplesPerPublish = (int)(publishFrequency/updatePeriod); /*sample and average then publish for filtering*/ + int publishCounter = 1; + double filterRatio = 0.3; + + while(1) { + + wait(updatePeriod); + start = end; + end = clock(); + time_between_readings = ((double)(end - start)) / CLOCKS_PER_SEC; + startcount = endcount; + endcount = GetCounts(); + ticks = endcount-startcount; + if(abs(ticks)>cpr/2) /***** for rollover case: *****/ + { ticks = ((ticks<0)-(ticks>0))*(cpr-abs(ticks)); } + velocity = filterRatio*((double)ticks)/cpr/time_between_readings + (1-filterRatio)*velocity; /* with filtering*/ + if(pc.readable()) + { + char abrahamsCommand = pc.getc(); + if(abrahamsCommand == '1'){ green = 1; } + if(abrahamsCommand == '0'){ green = 0; } + } + + printf("%f,%f,%f\n", motorCurrent.read(), motorVoltage.read(), velocity); + +// ---averaging filter--- + //if(publishCounter == samplesPerPublish){ +// printf("%f,%f,%f\n", motorCurrent.read(), motorVoltage.read(), velocity); +// publishCounter = 1; +// } +// publishCounter++; + + } + + +} + + + + + + + +