Abraham Gertler / Mbed 2 deprecated FlywheelInterface

For flywheel rig. Interfaces with matlab flywheel interface. 2.007 MIT Biomimetics Robotics Lab. Sangbae Kim. Ben Katz.

Dependencies:   mbed

main.cpp@12:f9f08c7551f4, 2017-04-14 (annotated)

Committer:
abraham1
Date:
Fri Apr 14 18:06:13 2017 +0000
Revision:
12:f9f08c7551f4
Parent:
11:ad41446b0edb 
Commented code;

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 12:f9f08c7551f4 3 // TODO: explain basic flow of run and data collection
abraham1 12:f9f08c7551f4 4
abraham1 0:dc5c88c2dd20 5 #include "mbed.h"
abraham1 0:dc5c88c2dd20 6 #include "time.h"
abraham1 3:df56bf381572 7 #include "stdio.h"
abraham1 3:df56bf381572 8 #include "ctype.h"
abraham1 0:dc5c88c2dd20 9
abraham1 4:f8a45966e63b 10 #define PI 3.14159265358979323846
abraham1 4:f8a45966e63b 11
abraham1 12:f9f08c7551f4 12 //Just for basic debugging
abraham1 12:f9f08c7551f4 13 //User button controls motor speed
abraham1 12:f9f08c7551f4 14 //Green LED should turn on while listening to pc for input before starting run
abraham1 0:dc5c88c2dd20 15 InterruptIn button(USER_BUTTON);
abraham1 3:df56bf381572 16 DigitalOut green(LED2);
abraham1 0:dc5c88c2dd20 17
abraham1 12:f9f08c7551f4 18 // Pololu VNH5019 Motor Driver Carrier. https://www.pololu.com/product/1451
abraham1 12:f9f08c7551f4 19 PwmOut pwm(D5); //pwm input on motor controller. do not use D3
abraham1 12:f9f08c7551f4 20 DigitalOut a(D2); //IN_A input on motor controller
abraham1 12:f9f08c7551f4 21 DigitalOut b(D4); //IN_B input on motor controller
abraham1 12:f9f08c7551f4 22
abraham1 12:f9f08c7551f4 23 //Hook up to Vout on current sensor
abraham1 12:f9f08c7551f4 24 //SparkFun Hall-Effect Current Sensor Breakout - ACS712
abraham1 12:f9f08c7551f4 25 //https://www.sparkfun.com/products/8882
abraham1 12:f9f08c7551f4 26 AnalogIn currentSense(A5);
abraham1 12:f9f08c7551f4 27
abraham1 12:f9f08c7551f4 28 //For communication with pc through matlab
abraham1 12:f9f08c7551f4 29 //Make sure baud rates are equal
abraham1 12:f9f08c7551f4 30 Serial pc(USBTX, USBRX, 115200);
abraham1 12:f9f08c7551f4 31
abraham1 12:f9f08c7551f4 32 const int CPR = 900*4; // Encoder counts per revolution (900). Change to match your encoder. x4 for quadrature
abraham1 12:f9f08c7551f4 33 const double VREF = 3; // Microcontroller reference voltage
abraham1 12:f9f08c7551f4 34 const float currentSensorOutputRatio = 0.185; // Volts/Amp specified by current sensor. Divide Voltage by cSenseOutput to get current
abraham1 12:f9f08c7551f4 35 const float PSupply_Voltage = 12.0; // Voltage input from power supply
abraham1 12:f9f08c7551f4 36 const float Output_Voltage = 2.0; // Desired output voltage to motor
abraham1 11:ad41446b0edb 37 const float pwm_flywheel = Output_Voltage/PSupply_Voltage;
abraham1 0:dc5c88c2dd20 38
abraham1 0:dc5c88c2dd20 39 void EncoderInitialise(void) {
abraham1 0:dc5c88c2dd20 40 // configure GPIO PA0 & PA1 as inputs for Encoder
abraham1 0:dc5c88c2dd20 41 RCC->AHB1ENR |= 0x00000001; // Enable clock for GPIOA
abraham1 0:dc5c88c2dd20 42
abraham1 0:dc5c88c2dd20 43 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 44 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 45 GPIOA->OSPEEDR |= 0x00000011;//|= GPIO_OSPEEDER_OSPEEDR0 | GPIO_OSPEEDER_OSPEEDR1 ; // Low speed /*!< GPIO port output speed register, Address offset: 0x08 */
abraham1 0:dc5c88c2dd20 46 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 47 GPIOA->AFR[0] |= 0x00000011 ; // AF01 for PA0 & PA1 /*!< GPIO alternate function registers, Address offset: 0x20-0x24 */
abraham1 0:dc5c88c2dd20 48 GPIOA->AFR[1] |= 0x00000000 ; // /*!< GPIO alternate function registers, Address offset: 0x20-0x24 */
abraham1 0:dc5c88c2dd20 49
abraham1 0:dc5c88c2dd20 50 // configure TIM2 as Encoder input
abraham1 0:dc5c88c2dd20 51 RCC->APB1ENR |= 0x00000001; // Enable clock for TIM2
abraham1 0:dc5c88c2dd20 52
abraham1 0:dc5c88c2dd20 53 TIM2->CR1 = 0x0001; // CEN(Counter Enable)='1' < TIM control register 1
abraham1 0:dc5c88c2dd20 54 TIM2->SMCR = 0x0003; // SMS='011' (Encoder mode 3) < TIM slave mode control register
abraham1 0:dc5c88c2dd20 55 TIM2->CCMR1 = 0x0101; // CC1S='01' CC2S='01' < TIM capture/compare mode register 1
abraham1 0:dc5c88c2dd20 56 TIM2->CCMR2 = 0x0000; // < TIM capture/compare mode register 2
abraham1 0:dc5c88c2dd20 57 TIM2->CCER = 0x0011; // CC1P CC2P < TIM capture/compare enable register
abraham1 0:dc5c88c2dd20 58 TIM2->PSC = 0x0000; // Prescaler = (0+1) < TIM prescaler
abraham1 4:f8a45966e63b 59 TIM2->ARR = CPR; // reload at CPR < TIM auto-reload register
abraham1 0:dc5c88c2dd20 60 TIM2->CNT = 0x0000; //reset the counter before we use it
abraham1 0:dc5c88c2dd20 61 }
abraham1 0:dc5c88c2dd20 62
abraham1 0:dc5c88c2dd20 63
abraham1 0:dc5c88c2dd20 64 //Zero encoder count//
abraham1 0:dc5c88c2dd20 65 void ZeroEncoder() {
abraham1 0:dc5c88c2dd20 66 TIM2->CNT=0 ; //reset timer count to zero
abraham1 0:dc5c88c2dd20 67 }
abraham1 0:dc5c88c2dd20 68
abraham1 0:dc5c88c2dd20 69 int GetCounts(void) {
abraham1 0:dc5c88c2dd20 70 int count = TIM2->CNT; //Read the timer count register
abraham1 0:dc5c88c2dd20 71 return count;
abraham1 0:dc5c88c2dd20 72 }
abraham1 0:dc5c88c2dd20 73
abraham1 0:dc5c88c2dd20 74 void pressed() {
abraham1 0:dc5c88c2dd20 75 float pwm_float = pwm.read();
abraham1 0:dc5c88c2dd20 76 int pwmV = (int)(100*pwm_float);
abraham1 0:dc5c88c2dd20 77 if(pwmV == 0){
abraham1 3:df56bf381572 78 pwm.write(0.05);
abraham1 3:df56bf381572 79 } else if (pwmV == 5){
abraham1 0:dc5c88c2dd20 80 pwm.write(0.2);
abraham1 0:dc5c88c2dd20 81 } else if (pwmV == 20){
abraham1 3:df56bf381572 82 pwm.write(0.75);
abraham1 3:df56bf381572 83 } else if (pwmV == 75){
abraham1 0:dc5c88c2dd20 84 pwm.write(0.0);
abraham1 3:df56bf381572 85 } else {
abraham1 3:df56bf381572 86 pwm.write(0.0);
abraham1 3:df56bf381572 87 }
abraham1 0:dc5c88c2dd20 88 }
abraham1 0:dc5c88c2dd20 89
abraham1 0:dc5c88c2dd20 90
abraham1 0:dc5c88c2dd20 91 int main() {
abraham1 0:dc5c88c2dd20 92
abraham1 12:f9f08c7551f4 93 int endcount, startcount; //encoder counts
abraham1 12:f9f08c7551f4 94 double time_between_readings; //between encoder readings
abraham1 3:df56bf381572 95 double velocity;
abraham1 3:df56bf381572 96 double currentSensed = 0;
abraham1 7:1726c40ad774 97 clock_t start, end, absoluteStart;
abraham1 0:dc5c88c2dd20 98 int ticks;
abraham1 0:dc5c88c2dd20 99 a=1; b=0; pwm.write(0);
abraham1 12:f9f08c7551f4 100 button.fall(&pressed); //adds pressed callback upon button push
abraham1 12:f9f08c7551f4 101
abraham1 12:f9f08c7551f4 102 /* we don't send all the information to matlab all the time. some collection and smoothing is done
abraham1 12:f9f08c7551f4 103 here in order to not overload matlab with input making it slow. And to take a lot of data so we
abraham1 12:f9f08c7551f4 104 can do smoothing quickly on this side */
abraham1 12:f9f08c7551f4 105 double updatePeriod = 0.01; /* must select carefully. too fast and you don't get enough encoder ticks*/
abraham1 12:f9f08c7551f4 106 double publishFrequency = 0.05; /* seconds. rate to publish to matlab. no need to overload matlab with input*/
abraham1 12:f9f08c7551f4 107 double samplesPerPublish = (int)(publishFrequency/updatePeriod); /*this improves time response of current filter and maintains smoothness*/
abraham1 0:dc5c88c2dd20 108 int publishCounter = 1;
abraham1 12:f9f08c7551f4 109
abraham1 12:f9f08c7551f4 110 /* the filter ratio on the speed data from encoder and on current input */
abraham1 10:ae139f40acc0 111 double filterRatio = 0.1;
abraham1 8:6ae3c3cd7f55 112 double currentFilterRatio = 0.035;
abraham1 12:f9f08c7551f4 113
abraham1 12:f9f08c7551f4 114 /* the current sensor has some resting value. record and subtract that out */
abraham1 1:f97adef77f4b 115 float currentSensorOffset = 0; int i;
abraham1 4:f8a45966e63b 116 for(i=1;i<301;i++){ currentSensorOffset += currentSense.read(); }
abraham1 4:f8a45966e63b 117 currentSensorOffset = currentSensorOffset*VREF/300;
abraham1 12:f9f08c7551f4 118
abraham1 1:f97adef77f4b 119 EncoderInitialise();
abraham1 12:f9f08c7551f4 120 fflush(pc); //clear any previous output
abraham1 1:f97adef77f4b 121
abraham1 11:ad41446b0edb 122
abraham1 0:dc5c88c2dd20 123 while(1) {
abraham1 5:f4c237d0bb32 124
abraham1 10:ae139f40acc0 125
abraham1 12:f9f08c7551f4 126
abraham1 12:f9f08c7551f4 127 while(1) { //Listen for PC input to start run
abraham1 5:f4c237d0bb32 128 green = true;
abraham1 11:ad41446b0edb 129 if(pc.readable()) {
abraham1 5:f4c237d0bb32 130 char charIn = pc.getc();
abraham1 6:73e417b1c521 131 if(charIn == 'g'){
abraham1 11:ad41446b0edb 132 fflush(pc);
abraham1 7:1726c40ad774 133 absoluteStart = clock();
abraham1 11:ad41446b0edb 134 end = clock();
abraham1 6:73e417b1c521 135 ZeroEncoder();
abraham1 6:73e417b1c521 136 velocity = 0;
abraham1 6:73e417b1c521 137 startcount = 0;
abraham1 6:73e417b1c521 138 endcount = 0;
abraham1 6:73e417b1c521 139 currentSensed = 0;
abraham1 11:ad41446b0edb 140 pwm.write(pwm_flywheel);
abraham1 6:73e417b1c521 141 break;
abraham1 6:73e417b1c521 142 }
abraham1 3:df56bf381572 143 }
abraham1 5:f4c237d0bb32 144 wait(0.05);
abraham1 5:f4c237d0bb32 145 }
abraham1 5:f4c237d0bb32 146
abraham1 5:f4c237d0bb32 147
abraham1 12:f9f08c7551f4 148
abraham1 5:f4c237d0bb32 149 while(1) {
abraham1 7:1726c40ad774 150
abraham1 5:f4c237d0bb32 151 green = false;
abraham1 5:f4c237d0bb32 152 wait(updatePeriod);
abraham1 5:f4c237d0bb32 153 start = end;
abraham1 5:f4c237d0bb32 154 end = clock();
abraham1 5:f4c237d0bb32 155 time_between_readings = ((double)(end - start)) / CLOCKS_PER_SEC;
abraham1 5:f4c237d0bb32 156 startcount = endcount;
abraham1 5:f4c237d0bb32 157 endcount = GetCounts();
abraham1 5:f4c237d0bb32 158 ticks = endcount-startcount;
abraham1 5:f4c237d0bb32 159 if(abs(ticks)>CPR/2) /***** for rollover case: *****/
abraham1 5:f4c237d0bb32 160 { ticks = ((ticks<0)-(ticks>0))*(CPR-abs(ticks)); }
abraham1 12:f9f08c7551f4 161
abraham1 5:f4c237d0bb32 162 velocity = filterRatio*((double)ticks)/CPR*2*PI/time_between_readings + (1-filterRatio)*velocity; /* with filtering*/
abraham1 5:f4c237d0bb32 163 currentSensed = currentFilterRatio*((double)currentSense.read()*VREF-currentSensorOffset) + (1-currentFilterRatio)*currentSensed;
abraham1 12:f9f08c7551f4 164
abraham1 12:f9f08c7551f4 165 if(pc.readable()) { //PC will send 'r' when run is finished
abraham1 5:f4c237d0bb32 166 char charIn = pc.getc();
abraham1 5:f4c237d0bb32 167 if(charIn == 'r'){
abraham1 11:ad41446b0edb 168 fflush(pc);
abraham1 12:f9f08c7551f4 169 pwm.write(0.0); //kill the motor
abraham1 12:f9f08c7551f4 170 break; //return to listening loop
abraham1 5:f4c237d0bb32 171 }
abraham1 5:f4c237d0bb32 172 }
abraham1 12:f9f08c7551f4 173
abraham1 12:f9f08c7551f4 174 if(publishCounter == samplesPerPublish) { //only publish once every samplesPerPublish
abraham1 11:ad41446b0edb 175 printf("%+9.5f,%+8.4f,%+9.4f,%+8.2f\n", currentSensed/currentSensorOutputRatio, pwm.read(), -velocity, ((double)(end-absoluteStart)/CLOCKS_PER_SEC));
abraham1 12:f9f08c7551f4 176 publishCounter = 1; //output formatting very important. running a simple string length checksum on matlab side to discard bad data
abraham1 7:1726c40ad774 177 }
abraham1 5:f4c237d0bb32 178 publishCounter++;
abraham1 5:f4c237d0bb32 179
abraham1 5:f4c237d0bb32 180 }
abraham1 5:f4c237d0bb32 181
abraham1 7:1726c40ad774 182
abraham1 0:dc5c88c2dd20 183 }
abraham1 0:dc5c88c2dd20 184
abraham1 0:dc5c88c2dd20 185 }
abraham1 0:dc5c88c2dd20 186
abraham1 0:dc5c88c2dd20 187
abraham1 0:dc5c88c2dd20 188
abraham1 0:dc5c88c2dd20 189
abraham1 0:dc5c88c2dd20 190
abraham1 0:dc5c88c2dd20 191
abraham1 0:dc5c88c2dd20 192
abraham1 0:dc5c88c2dd20 193