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@11:e563823d3c94, 2017-03-01 (annotated)

Committer:
abraham1
Date:
Wed Mar 01 03:10:29 2017 +0000
Revision:
11:e563823d3c94
Parent:
10:ae139f40acc0
Child:
12:6e13c34c4295
giving this to sangbae;

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 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 10:ae139f40acc0 19 const int CPR = 900*4; // Encoder counts per revolution. Change to match your encoder. x4 for quadrature!
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 11:e563823d3c94 22 const float PSupply_Voltage = 12.0;
abraham1 11:e563823d3c94 23 const float Output_Voltage = 6.0;
abraham1 11:e563823d3c94 24 const float pwm_pulley = Output_Voltage/PSupply_Voltage;
abraham1 0:dc5c88c2dd20 25
abraham1 0:dc5c88c2dd20 26 void EncoderInitialise(void) {
abraham1 0:dc5c88c2dd20 27 // configure GPIO PA0 & PA1 as inputs for Encoder
abraham1 0:dc5c88c2dd20 28 RCC->AHB1ENR |= 0x00000001; // Enable clock for GPIOA
abraham1 0:dc5c88c2dd20 29
abraham1 0:dc5c88c2dd20 30 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 31 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 32 GPIOA->OSPEEDR |= 0x00000011;//|= GPIO_OSPEEDER_OSPEEDR0 | GPIO_OSPEEDER_OSPEEDR1 ; // Low speed /*!< GPIO port output speed register, Address offset: 0x08 */
abraham1 0:dc5c88c2dd20 33 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 34 GPIOA->AFR[0] |= 0x00000011 ; // AF01 for PA0 & PA1 /*!< GPIO alternate function registers, Address offset: 0x20-0x24 */
abraham1 0:dc5c88c2dd20 35 GPIOA->AFR[1] |= 0x00000000 ; // /*!< GPIO alternate function registers, Address offset: 0x20-0x24 */
abraham1 0:dc5c88c2dd20 36
abraham1 0:dc5c88c2dd20 37 // configure TIM2 as Encoder input
abraham1 0:dc5c88c2dd20 38 RCC->APB1ENR |= 0x00000001; // Enable clock for TIM2
abraham1 0:dc5c88c2dd20 39
abraham1 0:dc5c88c2dd20 40 TIM2->CR1 = 0x0001; // CEN(Counter Enable)='1' < TIM control register 1
abraham1 0:dc5c88c2dd20 41 TIM2->SMCR = 0x0003; // SMS='011' (Encoder mode 3) < TIM slave mode control register
abraham1 0:dc5c88c2dd20 42 TIM2->CCMR1 = 0x0101; // CC1S='01' CC2S='01' < TIM capture/compare mode register 1
abraham1 0:dc5c88c2dd20 43 TIM2->CCMR2 = 0x0000; // < TIM capture/compare mode register 2
abraham1 0:dc5c88c2dd20 44 TIM2->CCER = 0x0011; // CC1P CC2P < TIM capture/compare enable register
abraham1 0:dc5c88c2dd20 45 TIM2->PSC = 0x0000; // Prescaler = (0+1) < TIM prescaler
abraham1 4:f8a45966e63b 46 TIM2->ARR = CPR; // reload at CPR < TIM auto-reload register
abraham1 0:dc5c88c2dd20 47 TIM2->CNT = 0x0000; //reset the counter before we use it
abraham1 0:dc5c88c2dd20 48 }
abraham1 0:dc5c88c2dd20 49
abraham1 0:dc5c88c2dd20 50
abraham1 0:dc5c88c2dd20 51 //Zero encoder count//
abraham1 0:dc5c88c2dd20 52 void ZeroEncoder() {
abraham1 0:dc5c88c2dd20 53 TIM2->CNT=0 ; //reset timer count to zero
abraham1 0:dc5c88c2dd20 54 }
abraham1 0:dc5c88c2dd20 55
abraham1 0:dc5c88c2dd20 56 int GetCounts(void) {
abraham1 0:dc5c88c2dd20 57 int count = TIM2->CNT; //Read the timer count register
abraham1 0:dc5c88c2dd20 58 return count;
abraham1 0:dc5c88c2dd20 59 }
abraham1 0:dc5c88c2dd20 60
abraham1 0:dc5c88c2dd20 61 void pressed() {
abraham1 0:dc5c88c2dd20 62 float pwm_float = pwm.read();
abraham1 0:dc5c88c2dd20 63 int pwmV = (int)(100*pwm_float);
abraham1 0:dc5c88c2dd20 64 if(pwmV == 0){
abraham1 3:df56bf381572 65 pwm.write(0.05);
abraham1 3:df56bf381572 66 } else if (pwmV == 5){
abraham1 0:dc5c88c2dd20 67 pwm.write(0.2);
abraham1 0:dc5c88c2dd20 68 } else if (pwmV == 20){
abraham1 3:df56bf381572 69 pwm.write(0.75);
abraham1 3:df56bf381572 70 } else if (pwmV == 75){
abraham1 0:dc5c88c2dd20 71 pwm.write(0.0);
abraham1 3:df56bf381572 72 } else {
abraham1 3:df56bf381572 73 pwm.write(0.0);
abraham1 3:df56bf381572 74 }
abraham1 0:dc5c88c2dd20 75 }
abraham1 0:dc5c88c2dd20 76
abraham1 0:dc5c88c2dd20 77
abraham1 0:dc5c88c2dd20 78 int main() {
abraham1 0:dc5c88c2dd20 79
abraham1 3:df56bf381572 80 int endcount, startcount;
abraham1 3:df56bf381572 81 double time_between_readings;
abraham1 3:df56bf381572 82 double velocity;
abraham1 3:df56bf381572 83 double currentSensed = 0;
abraham1 7:1726c40ad774 84 clock_t start, end, absoluteStart;
abraham1 0:dc5c88c2dd20 85 int ticks;
abraham1 0:dc5c88c2dd20 86 a=1; b=0; pwm.write(0);
abraham1 0:dc5c88c2dd20 87 button.fall(&pressed);
abraham1 0:dc5c88c2dd20 88 double updatePeriod = 0.01; /* must select carefully */
abraham1 4:f8a45966e63b 89 double publishFrequency = 0.05; /* seconds. rate to publish to matlab */
abraham1 1:f97adef77f4b 90 double samplesPerPublish = (int)(publishFrequency/updatePeriod); /*this improves time response of filter and maintains smoothness*/
abraham1 0:dc5c88c2dd20 91 int publishCounter = 1;
abraham1 10:ae139f40acc0 92 double filterRatio = 0.1;
abraham1 8:6ae3c3cd7f55 93 double currentFilterRatio = 0.035;
abraham1 1:f97adef77f4b 94 float currentSensorOffset = 0; int i;
abraham1 4:f8a45966e63b 95 for(i=1;i<301;i++){ currentSensorOffset += currentSense.read(); }
abraham1 4:f8a45966e63b 96 currentSensorOffset = currentSensorOffset*VREF/300;
abraham1 1:f97adef77f4b 97 EncoderInitialise();
abraham1 1:f97adef77f4b 98 fflush(pc);
abraham1 1:f97adef77f4b 99
abraham1 11:e563823d3c94 100
abraham1 0:dc5c88c2dd20 101 while(1) {
abraham1 5:f4c237d0bb32 102
abraham1 10:ae139f40acc0 103
abraham1 5:f4c237d0bb32 104 while(1) {
abraham1 7:1726c40ad774 105
abraham1 5:f4c237d0bb32 106 green = true;
abraham1 11:e563823d3c94 107 if(pc.readable()) {
abraham1 5:f4c237d0bb32 108 char charIn = pc.getc();
abraham1 6:73e417b1c521 109 if(charIn == 'g'){
abraham1 11:e563823d3c94 110 fflush(pc);
abraham1 7:1726c40ad774 111 absoluteStart = clock();
abraham1 11:e563823d3c94 112 end = clock();
abraham1 6:73e417b1c521 113 ZeroEncoder();
abraham1 6:73e417b1c521 114 velocity = 0;
abraham1 6:73e417b1c521 115 startcount = 0;
abraham1 6:73e417b1c521 116 endcount = 0;
abraham1 6:73e417b1c521 117 currentSensed = 0;
abraham1 11:e563823d3c94 118 pwm.write(pwm_pulley);
abraham1 6:73e417b1c521 119 break;
abraham1 6:73e417b1c521 120 }
abraham1 3:df56bf381572 121 }
abraham1 11:e563823d3c94 122
abraham1 5:f4c237d0bb32 123 wait(0.05);
abraham1 7:1726c40ad774 124
abraham1 5:f4c237d0bb32 125 }
abraham1 5:f4c237d0bb32 126
abraham1 5:f4c237d0bb32 127
abraham1 5:f4c237d0bb32 128 while(1) {
abraham1 7:1726c40ad774 129
abraham1 5:f4c237d0bb32 130 green = false;
abraham1 5:f4c237d0bb32 131 wait(updatePeriod);
abraham1 5:f4c237d0bb32 132 start = end;
abraham1 5:f4c237d0bb32 133 end = clock();
abraham1 5:f4c237d0bb32 134 time_between_readings = ((double)(end - start)) / CLOCKS_PER_SEC;
abraham1 5:f4c237d0bb32 135 startcount = endcount;
abraham1 5:f4c237d0bb32 136 endcount = GetCounts();
abraham1 5:f4c237d0bb32 137 ticks = endcount-startcount;
abraham1 5:f4c237d0bb32 138 if(abs(ticks)>CPR/2) /***** for rollover case: *****/
abraham1 5:f4c237d0bb32 139 { ticks = ((ticks<0)-(ticks>0))*(CPR-abs(ticks)); }
abraham1 5:f4c237d0bb32 140 velocity = filterRatio*((double)ticks)/CPR*2*PI/time_between_readings + (1-filterRatio)*velocity; /* with filtering*/
abraham1 5:f4c237d0bb32 141 currentSensed = currentFilterRatio*((double)currentSense.read()*VREF-currentSensorOffset) + (1-currentFilterRatio)*currentSensed;
abraham1 5:f4c237d0bb32 142 if(pc.readable())
abraham1 0:dc5c88c2dd20 143 {
abraham1 5:f4c237d0bb32 144 char charIn = pc.getc();
abraham1 5:f4c237d0bb32 145 if(charIn == 'r'){
abraham1 11:e563823d3c94 146 fflush(pc);
abraham1 11:e563823d3c94 147 pwm.write(0.0);
abraham1 5:f4c237d0bb32 148 break;
abraham1 5:f4c237d0bb32 149 }
abraham1 5:f4c237d0bb32 150 }
abraham1 7:1726c40ad774 151 if(publishCounter == samplesPerPublish) {
abraham1 11:e563823d3c94 152 printf("%+9.5f,%+8.4f,%+9.4f,%+8.2f\n", currentSensed/currentSensorOutputRatio, pwm.read(), velocity, ((double)(end-absoluteStart)/CLOCKS_PER_SEC));
abraham1 11:e563823d3c94 153 publishCounter = 1; // good for 1000 seconds. and room to grow one power of 10 on all other inputs. //just changed velocity to 4 precision to give it extra room for no load? bug maybe?
abraham1 7:1726c40ad774 154 }
abraham1 5:f4c237d0bb32 155 publishCounter++;
abraham1 5:f4c237d0bb32 156
abraham1 5:f4c237d0bb32 157 }
abraham1 5:f4c237d0bb32 158
abraham1 7:1726c40ad774 159
abraham1 0:dc5c88c2dd20 160 }
abraham1 0:dc5c88c2dd20 161
abraham1 0:dc5c88c2dd20 162 }
abraham1 0:dc5c88c2dd20 163
abraham1 0:dc5c88c2dd20 164
abraham1 0:dc5c88c2dd20 165
abraham1 0:dc5c88c2dd20 166
abraham1 0:dc5c88c2dd20 167
abraham1 0:dc5c88c2dd20 168
abraham1 0:dc5c88c2dd20 169
abraham1 0:dc5c88c2dd20 170