2.007 PulleyInterface mbed code. Biomimetics robotics lab. Sangbae Kim. Ben Katz. For use with PulleyInterface.mlapp
main.cpp
- Committer:
- abraham1
- Date:
- 2017-02-03
- Revision:
- 1:f97adef77f4b
- Parent:
- 0:dc5c88c2dd20
- Child:
- 2:a7100c183940
File content as of revision 1:f97adef77f4b:
///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() { const double vRef = 3.3; 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); /*this improves time response of filter and maintains smoothness*/ int publishCounter = 1; double filterRatio = 0.05; float currentSensorOffset = 0; int i; for(i=1;i<101;i++){ currentSensorOffset += motorCurrent.read(); } currentSensorOffset = currentSensorOffset*vRef/100; EncoderInitialise(); fflush(pc); 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; } } if(publishCounter == samplesPerPublish){ printf("%f,%f,%f\n", motorCurrent.read()*vRef-(float)currentSensorOffset, motorVoltage.read(), velocity); publishCounter = 1; } publishCounter++; } }