For flywheel rig. Interfaces with matlab flywheel interface. 2.007 MIT Biomimetics Robotics Lab. Sangbae Kim. Ben Katz.
main.cpp@5:f4c237d0bb32, 2017-02-06 (annotated)
- Committer:
- abraham1
- Date:
- Mon Feb 06 20:03:40 2017 +0000
- Revision:
- 5:f4c237d0bb32
- Parent:
- 4:f8a45966e63b
- Child:
- 6:73e417b1c521
things printing right, and incoming right, but matlab is not graphing at all
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 | 3:df56bf381572 | 5 | #include "stdio.h" |
abraham1 | 3:df56bf381572 | 6 | #include "ctype.h" |
abraham1 | 0:dc5c88c2dd20 | 7 | |
abraham1 | 4:f8a45966e63b | 8 | #define PI 3.14159265358979323846 |
abraham1 | 4:f8a45966e63b | 9 | |
abraham1 | 0:dc5c88c2dd20 | 10 | InterruptIn button(USER_BUTTON); |
abraham1 | 0:dc5c88c2dd20 | 11 | PwmOut pwm(D5);//do not use D3 |
abraham1 | 0:dc5c88c2dd20 | 12 | DigitalOut a(D2); |
abraham1 | 0:dc5c88c2dd20 | 13 | DigitalOut b(D4); |
abraham1 | 0:dc5c88c2dd20 | 14 | |
abraham1 | 3:df56bf381572 | 15 | AnalogIn currentSense(A5); //hook up to Vout on current sensor |
abraham1 | 3:df56bf381572 | 16 | Serial pc(USBTX, USBRX, 115200); |
abraham1 | 3:df56bf381572 | 17 | DigitalOut green(LED2); |
abraham1 | 0:dc5c88c2dd20 | 18 | |
abraham1 | 4:f8a45966e63b | 19 | const int CPR = 900; // Encoder counts per revolution. Change to match your encoder |
abraham1 | 4:f8a45966e63b | 20 | const double VREF = 3; //Microcontroller reference voltage |
abraham1 | 4:f8a45966e63b | 21 | const float currentSensorOutputRatio = 0.185; // Volts/Amp. Divide Voltage by cSenseOutput to get current |
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 | 4:f8a45966e63b | 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 | if(pwmV == 0){ |
abraham1 | 3:df56bf381572 | 62 | pwm.write(0.05); |
abraham1 | 3:df56bf381572 | 63 | } else if (pwmV == 5){ |
abraham1 | 0:dc5c88c2dd20 | 64 | pwm.write(0.2); |
abraham1 | 0:dc5c88c2dd20 | 65 | } else if (pwmV == 20){ |
abraham1 | 3:df56bf381572 | 66 | pwm.write(0.75); |
abraham1 | 3:df56bf381572 | 67 | } else if (pwmV == 75){ |
abraham1 | 0:dc5c88c2dd20 | 68 | pwm.write(0.0); |
abraham1 | 3:df56bf381572 | 69 | } else { |
abraham1 | 3:df56bf381572 | 70 | pwm.write(0.0); |
abraham1 | 3:df56bf381572 | 71 | } |
abraham1 | 0:dc5c88c2dd20 | 72 | } |
abraham1 | 0:dc5c88c2dd20 | 73 | |
abraham1 | 0:dc5c88c2dd20 | 74 | |
abraham1 | 0:dc5c88c2dd20 | 75 | int main() { |
abraham1 | 0:dc5c88c2dd20 | 76 | |
abraham1 | 3:df56bf381572 | 77 | int endcount, startcount; |
abraham1 | 3:df56bf381572 | 78 | double time_between_readings; |
abraham1 | 3:df56bf381572 | 79 | double velocity; |
abraham1 | 3:df56bf381572 | 80 | double currentSensed = 0; |
abraham1 | 5:f4c237d0bb32 | 81 | clock_t start, end; |
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 | 4:f8a45966e63b | 86 | double publishFrequency = 0.05; /* seconds. rate to publish to matlab */ |
abraham1 | 1:f97adef77f4b | 87 | double samplesPerPublish = (int)(publishFrequency/updatePeriod); /*this improves time response of filter and maintains smoothness*/ |
abraham1 | 0:dc5c88c2dd20 | 88 | int publishCounter = 1; |
abraham1 | 4:f8a45966e63b | 89 | double filterRatio = 0.045; |
abraham1 | 3:df56bf381572 | 90 | double currentFilterRatio = 0.02; |
abraham1 | 1:f97adef77f4b | 91 | float currentSensorOffset = 0; int i; |
abraham1 | 4:f8a45966e63b | 92 | for(i=1;i<301;i++){ currentSensorOffset += currentSense.read(); } |
abraham1 | 4:f8a45966e63b | 93 | currentSensorOffset = currentSensorOffset*VREF/300; |
abraham1 | 1:f97adef77f4b | 94 | |
abraham1 | 1:f97adef77f4b | 95 | EncoderInitialise(); |
abraham1 | 1:f97adef77f4b | 96 | fflush(pc); |
abraham1 | 1:f97adef77f4b | 97 | |
abraham1 | 4:f8a45966e63b | 98 | /*** wait here for matlab information like voltage before starting? ***/ |
abraham1 | 1:f97adef77f4b | 99 | |
abraham1 | 0:dc5c88c2dd20 | 100 | while(1) { |
abraham1 | 5:f4c237d0bb32 | 101 | |
abraham1 | 5:f4c237d0bb32 | 102 | while(1) { |
abraham1 | 5:f4c237d0bb32 | 103 | green = true; |
abraham1 | 5:f4c237d0bb32 | 104 | if(pc.readable()) |
abraham1 | 5:f4c237d0bb32 | 105 | { |
abraham1 | 5:f4c237d0bb32 | 106 | char charIn = pc.getc(); |
abraham1 | 5:f4c237d0bb32 | 107 | if(charIn == 'g'){ end = clock(); break; } |
abraham1 | 3:df56bf381572 | 108 | } |
abraham1 | 5:f4c237d0bb32 | 109 | wait(0.05); |
abraham1 | 5:f4c237d0bb32 | 110 | } |
abraham1 | 5:f4c237d0bb32 | 111 | |
abraham1 | 5:f4c237d0bb32 | 112 | |
abraham1 | 5:f4c237d0bb32 | 113 | while(1) { |
abraham1 | 5:f4c237d0bb32 | 114 | green = false; |
abraham1 | 5:f4c237d0bb32 | 115 | wait(updatePeriod); |
abraham1 | 5:f4c237d0bb32 | 116 | clock_t absoluteStart = clock(); |
abraham1 | 5:f4c237d0bb32 | 117 | start = end; |
abraham1 | 5:f4c237d0bb32 | 118 | end = clock(); |
abraham1 | 5:f4c237d0bb32 | 119 | time_between_readings = ((double)(end - start)) / CLOCKS_PER_SEC; |
abraham1 | 5:f4c237d0bb32 | 120 | startcount = endcount; |
abraham1 | 5:f4c237d0bb32 | 121 | endcount = GetCounts(); |
abraham1 | 5:f4c237d0bb32 | 122 | ticks = endcount-startcount; |
abraham1 | 5:f4c237d0bb32 | 123 | if(abs(ticks)>CPR/2) /***** for rollover case: *****/ |
abraham1 | 5:f4c237d0bb32 | 124 | { ticks = ((ticks<0)-(ticks>0))*(CPR-abs(ticks)); } |
abraham1 | 5:f4c237d0bb32 | 125 | velocity = filterRatio*((double)ticks)/CPR*2*PI/time_between_readings + (1-filterRatio)*velocity; /* with filtering*/ |
abraham1 | 5:f4c237d0bb32 | 126 | currentSensed = currentFilterRatio*((double)currentSense.read()*VREF-currentSensorOffset) + (1-currentFilterRatio)*currentSensed; |
abraham1 | 5:f4c237d0bb32 | 127 | if(pc.readable()) |
abraham1 | 0:dc5c88c2dd20 | 128 | { |
abraham1 | 5:f4c237d0bb32 | 129 | char charIn = pc.getc(); |
abraham1 | 5:f4c237d0bb32 | 130 | if(charIn == 'r'){ |
abraham1 | 5:f4c237d0bb32 | 131 | fflush(pc); /* TODO: purge much better than this! */ |
abraham1 | 5:f4c237d0bb32 | 132 | // green = !green; |
abraham1 | 5:f4c237d0bb32 | 133 | break; |
abraham1 | 5:f4c237d0bb32 | 134 | } else if(isdigit(charIn)) { |
abraham1 | 5:f4c237d0bb32 | 135 | double abrahamsCommand = (double)(charIn - '0'); |
abraham1 | 5:f4c237d0bb32 | 136 | pwm.write(abrahamsCommand/10.0); |
abraham1 | 5:f4c237d0bb32 | 137 | } |
abraham1 | 5:f4c237d0bb32 | 138 | } |
abraham1 | 5:f4c237d0bb32 | 139 | if(publishCounter == samplesPerPublish) |
abraham1 | 5:f4c237d0bb32 | 140 | { |
abraham1 | 5:f4c237d0bb32 | 141 | printf("%+8f,%+8f,%+8f,%+8f\n", currentSensed/currentSensorOutputRatio, pwm.read(), velocity, ((double)(end-absoluteStart)/CLOCKS_PER_SEC)); |
abraham1 | 5:f4c237d0bb32 | 142 | publishCounter = 1; |
abraham1 | 5:f4c237d0bb32 | 143 | } |
abraham1 | 5:f4c237d0bb32 | 144 | publishCounter++; |
abraham1 | 5:f4c237d0bb32 | 145 | |
abraham1 | 5:f4c237d0bb32 | 146 | } |
abraham1 | 5:f4c237d0bb32 | 147 | |
abraham1 | 0:dc5c88c2dd20 | 148 | } |
abraham1 | 0:dc5c88c2dd20 | 149 | |
abraham1 | 0:dc5c88c2dd20 | 150 | } |
abraham1 | 0:dc5c88c2dd20 | 151 | |
abraham1 | 0:dc5c88c2dd20 | 152 | |
abraham1 | 0:dc5c88c2dd20 | 153 | |
abraham1 | 0:dc5c88c2dd20 | 154 | |
abraham1 | 0:dc5c88c2dd20 | 155 | |
abraham1 | 0:dc5c88c2dd20 | 156 | |
abraham1 | 0:dc5c88c2dd20 | 157 | |
abraham1 | 0:dc5c88c2dd20 | 158 |