Abraham Gertler / Mbed 2 deprecated PulleyInterface

2.007 PulleyInterface mbed code. Biomimetics robotics lab. Sangbae Kim. Ben Katz. For use with PulleyInterface.mlapp

Dependencies:   mbed

main.cpp@0:dc5c88c2dd20, 2017-02-03 (annotated)

Committer:
abraham1
Date:
Fri Feb 03 18:18:22 2017 +0000
Revision:
0:dc5c88c2dd20
Child:
1:f97adef77f4b
1st Order Low Pass

Who changed what in which revision?

UserRevisionLine numberNew 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 0:dc5c88c2dd20 79 EncoderInitialise();
abraham1 0:dc5c88c2dd20 80 clock_t start;
abraham1 0:dc5c88c2dd20 81 clock_t end = clock();
abraham1 0:dc5c88c2dd20 82 int ticks;
abraham1 0:dc5c88c2dd20 83 a=1; b=0; pwm.write(0);
abraham1 0:dc5c88c2dd20 84 button.fall(&pressed);
abraham1 0:dc5c88c2dd20 85 double updatePeriod = 0.01; /* must select carefully */
abraham1 0:dc5c88c2dd20 86 double publishFrequency = 0.1; /* seconds. rate to publish to matlab */
abraham1 0:dc5c88c2dd20 87 double samplesPerPublish = (int)(publishFrequency/updatePeriod); /*sample and average then publish for filtering*/
abraham1 0:dc5c88c2dd20 88 int publishCounter = 1;
abraham1 0:dc5c88c2dd20 89 double filterRatio = 0.3;
abraham1 0:dc5c88c2dd20 90
abraham1 0:dc5c88c2dd20 91 while(1) {
abraham1 0:dc5c88c2dd20 92
abraham1 0:dc5c88c2dd20 93 wait(updatePeriod);
abraham1 0:dc5c88c2dd20 94 start = end;
abraham1 0:dc5c88c2dd20 95 end = clock();
abraham1 0:dc5c88c2dd20 96 time_between_readings = ((double)(end - start)) / CLOCKS_PER_SEC;
abraham1 0:dc5c88c2dd20 97 startcount = endcount;
abraham1 0:dc5c88c2dd20 98 endcount = GetCounts();
abraham1 0:dc5c88c2dd20 99 ticks = endcount-startcount;
abraham1 0:dc5c88c2dd20 100 if(abs(ticks)>cpr/2) /***** for rollover case: *****/
abraham1 0:dc5c88c2dd20 101 { ticks = ((ticks<0)-(ticks>0))*(cpr-abs(ticks)); }
abraham1 0:dc5c88c2dd20 102 velocity = filterRatio*((double)ticks)/cpr/time_between_readings + (1-filterRatio)*velocity; /* with filtering*/
abraham1 0:dc5c88c2dd20 103 if(pc.readable())
abraham1 0:dc5c88c2dd20 104 {
abraham1 0:dc5c88c2dd20 105 char abrahamsCommand = pc.getc();
abraham1 0:dc5c88c2dd20 106 if(abrahamsCommand == '1'){ green = 1; }
abraham1 0:dc5c88c2dd20 107 if(abrahamsCommand == '0'){ green = 0; }
abraham1 0:dc5c88c2dd20 108 }
abraham1 0:dc5c88c2dd20 109
abraham1 0:dc5c88c2dd20 110 printf("%f,%f,%f\n", motorCurrent.read(), motorVoltage.read(), velocity);
abraham1 0:dc5c88c2dd20 111
abraham1 0:dc5c88c2dd20 112 // ---averaging filter---
abraham1 0:dc5c88c2dd20 113 //if(publishCounter == samplesPerPublish){
abraham1 0:dc5c88c2dd20 114 // printf("%f,%f,%f\n", motorCurrent.read(), motorVoltage.read(), velocity);
abraham1 0:dc5c88c2dd20 115 // publishCounter = 1;
abraham1 0:dc5c88c2dd20 116 // }
abraham1 0:dc5c88c2dd20 117 // publishCounter++;
abraham1 0:dc5c88c2dd20 118
abraham1 0:dc5c88c2dd20 119 }
abraham1 0:dc5c88c2dd20 120
abraham1 0:dc5c88c2dd20 121
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