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@13:d9d0eb9103d3, 2017-04-15 (annotated)

Committer:
abraham1
Date:
Sat Apr 15 13:48:41 2017 +0000
Revision:
13:d9d0eb9103d3
Parent:
12:6e13c34c4295 
added contact information;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
abraham1 12:6e13c34c4295 1
abraham1 12:6e13c34c4295 2
abraham1 12:6e13c34c4295 3 /***
abraham1 12:6e13c34c4295 4 Basic Program Flow:
abraham1 12:6e13c34c4295 5 mbed turns on. initializes.
abraham1 12:6e13c34c4295 6 enters main while loop then first sub while loop where it listens for a go command
abraham1 12:6e13c34c4295 7 mbed recieves go commmand, initializes for data collection, breaks out of first sub while
abraham1 12:6e13c34c4295 8 enters second sub while. starts collecting and publishing data, listening for stop
abraham1 12:6e13c34c4295 9 mbed hears stop, breaks out of second while loop, returns back to first while loop
abraham1 12:6e13c34c4295 10
abraham1 12:6e13c34c4295 11
abraham1 12:6e13c34c4295 12 Keep in Mind:
abraham1 12:6e13c34c4295 13 Serial communication between mbed matlab sometimes sucks, and often breaks down
abraham1 12:6e13c34c4295 14 Most of the things in this program (and the corresponding matlab one) are meant
abraham1 12:6e13c34c4295 15 to minimize the amount of bad data that gets passed over serial and maintain the
abraham1 12:6e13c34c4295 16 connection.
abraham1 13:d9d0eb9103d3 17
abraham1 13:d9d0eb9103d3 18 Abraham Gertler
abraham1 13:d9d0eb9103d3 19 MIT MechE 2018
abraham1 13:d9d0eb9103d3 20 gertler@mit.edu
abraham1 12:6e13c34c4295 21 ***/
abraham1 12:6e13c34c4295 22
abraham1 0:dc5c88c2dd20 23
abraham1 0:dc5c88c2dd20 24 #include "mbed.h"
abraham1 0:dc5c88c2dd20 25 #include "time.h"
abraham1 3:df56bf381572 26 #include "stdio.h"
abraham1 3:df56bf381572 27 #include "ctype.h"
abraham1 0:dc5c88c2dd20 28
abraham1 4:f8a45966e63b 29 #define PI 3.14159265358979323846
abraham1 4:f8a45966e63b 30
abraham1 12:6e13c34c4295 31
abraham1 12:6e13c34c4295 32 //Just for basic debugging
abraham1 12:6e13c34c4295 33 //User button controls motor speed
abraham1 12:6e13c34c4295 34 //Green LED should turn on while listening to pc for input before starting run
abraham1 0:dc5c88c2dd20 35 InterruptIn button(USER_BUTTON);
abraham1 3:df56bf381572 36 DigitalOut green(LED2);
abraham1 0:dc5c88c2dd20 37
abraham1 12:6e13c34c4295 38 // Pololu VNH5019 Motor Driver Carrier. https://www.pololu.com/product/1451
abraham1 12:6e13c34c4295 39 PwmOut pwm(D5); //pwm input on motor controller. do not use D3
abraham1 12:6e13c34c4295 40 DigitalOut a(D2); //IN_A input on motor controller
abraham1 12:6e13c34c4295 41 DigitalOut b(D4); //IN_B input on motor controller
abraham1 12:6e13c34c4295 42
abraham1 12:6e13c34c4295 43 //Hook up to Vout on current sensor
abraham1 12:6e13c34c4295 44 //SparkFun Hall-Effect Current Sensor Breakout - ACS712
abraham1 12:6e13c34c4295 45 //https://www.sparkfun.com/products/8882
abraham1 12:6e13c34c4295 46 AnalogIn currentSense(A5);
abraham1 12:6e13c34c4295 47
abraham1 12:6e13c34c4295 48 //For communication with pc through matlab
abraham1 12:6e13c34c4295 49 //Make sure baud rates are equal
abraham1 12:6e13c34c4295 50 Serial pc(USBTX, USBRX, 115200);
abraham1 12:6e13c34c4295 51
abraham1 12:6e13c34c4295 52 const int CPR = 900*4; // Encoder counts per revolution (900). Change to match your encoder. x4 for quadrature
abraham1 12:6e13c34c4295 53 const double VREF = 3; // Microcontroller reference voltage
abraham1 12:6e13c34c4295 54 const float currentSensorOutputRatio = 0.185; // Volts/Amp specified by current sensor. Divide Voltage by cSenseOutput to get current
abraham1 12:6e13c34c4295 55 const float PSupply_Voltage = 12.0; // Voltage input from powersupply
abraham1 12:6e13c34c4295 56 const float Output_Voltage = 6.0; // Maximum output voltage desired
abraham1 11:e563823d3c94 57 const float pwm_pulley = Output_Voltage/PSupply_Voltage;
abraham1 0:dc5c88c2dd20 58
abraham1 12:6e13c34c4295 59
abraham1 12:6e13c34c4295 60 ///setup code for encoder on ***** pins PA0 and PA1 (A0 and A1) ****** ///
abraham1 0:dc5c88c2dd20 61 void EncoderInitialise(void) {
abraham1 0:dc5c88c2dd20 62 // configure GPIO PA0 & PA1 as inputs for Encoder
abraham1 0:dc5c88c2dd20 63 RCC->AHB1ENR |= 0x00000001; // Enable clock for GPIOA
abraham1 0:dc5c88c2dd20 64
abraham1 0:dc5c88c2dd20 65 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 66 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 67 GPIOA->OSPEEDR |= 0x00000011;//|= GPIO_OSPEEDER_OSPEEDR0 | GPIO_OSPEEDER_OSPEEDR1 ; // Low speed /*!< GPIO port output speed register, Address offset: 0x08 */
abraham1 0:dc5c88c2dd20 68 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 69 GPIOA->AFR[0] |= 0x00000011 ; // AF01 for PA0 & PA1 /*!< GPIO alternate function registers, Address offset: 0x20-0x24 */
abraham1 0:dc5c88c2dd20 70 GPIOA->AFR[1] |= 0x00000000 ; // /*!< GPIO alternate function registers, Address offset: 0x20-0x24 */
abraham1 0:dc5c88c2dd20 71
abraham1 0:dc5c88c2dd20 72 // configure TIM2 as Encoder input
abraham1 0:dc5c88c2dd20 73 RCC->APB1ENR |= 0x00000001; // Enable clock for TIM2
abraham1 0:dc5c88c2dd20 74
abraham1 0:dc5c88c2dd20 75 TIM2->CR1 = 0x0001; // CEN(Counter Enable)='1' < TIM control register 1
abraham1 0:dc5c88c2dd20 76 TIM2->SMCR = 0x0003; // SMS='011' (Encoder mode 3) < TIM slave mode control register
abraham1 0:dc5c88c2dd20 77 TIM2->CCMR1 = 0x0101; // CC1S='01' CC2S='01' < TIM capture/compare mode register 1
abraham1 0:dc5c88c2dd20 78 TIM2->CCMR2 = 0x0000; // < TIM capture/compare mode register 2
abraham1 0:dc5c88c2dd20 79 TIM2->CCER = 0x0011; // CC1P CC2P < TIM capture/compare enable register
abraham1 0:dc5c88c2dd20 80 TIM2->PSC = 0x0000; // Prescaler = (0+1) < TIM prescaler
abraham1 4:f8a45966e63b 81 TIM2->ARR = CPR; // reload at CPR < TIM auto-reload register
abraham1 0:dc5c88c2dd20 82 TIM2->CNT = 0x0000; //reset the counter before we use it
abraham1 0:dc5c88c2dd20 83 }
abraham1 0:dc5c88c2dd20 84
abraham1 0:dc5c88c2dd20 85
abraham1 0:dc5c88c2dd20 86 //Zero encoder count//
abraham1 0:dc5c88c2dd20 87 void ZeroEncoder() {
abraham1 0:dc5c88c2dd20 88 TIM2->CNT=0 ; //reset timer count to zero
abraham1 0:dc5c88c2dd20 89 }
abraham1 0:dc5c88c2dd20 90
abraham1 0:dc5c88c2dd20 91 int GetCounts(void) {
abraham1 0:dc5c88c2dd20 92 int count = TIM2->CNT; //Read the timer count register
abraham1 0:dc5c88c2dd20 93 return count;
abraham1 0:dc5c88c2dd20 94 }
abraham1 0:dc5c88c2dd20 95
abraham1 0:dc5c88c2dd20 96 void pressed() {
abraham1 0:dc5c88c2dd20 97 float pwm_float = pwm.read();
abraham1 0:dc5c88c2dd20 98 int pwmV = (int)(100*pwm_float);
abraham1 0:dc5c88c2dd20 99 if(pwmV == 0){
abraham1 3:df56bf381572 100 pwm.write(0.05);
abraham1 3:df56bf381572 101 } else if (pwmV == 5){
abraham1 0:dc5c88c2dd20 102 pwm.write(0.2);
abraham1 0:dc5c88c2dd20 103 } else if (pwmV == 20){
abraham1 3:df56bf381572 104 pwm.write(0.75);
abraham1 3:df56bf381572 105 } else if (pwmV == 75){
abraham1 0:dc5c88c2dd20 106 pwm.write(0.0);
abraham1 3:df56bf381572 107 } else {
abraham1 3:df56bf381572 108 pwm.write(0.0);
abraham1 3:df56bf381572 109 }
abraham1 0:dc5c88c2dd20 110 }
abraham1 0:dc5c88c2dd20 111
abraham1 0:dc5c88c2dd20 112
abraham1 0:dc5c88c2dd20 113 int main() {
abraham1 0:dc5c88c2dd20 114
abraham1 12:6e13c34c4295 115 int endcount, startcount; // encoder counts
abraham1 12:6e13c34c4295 116 double time_between_readings; // between encoder readings
abraham1 12:6e13c34c4295 117 double velocity; // radians/time
abraham1 3:df56bf381572 118 double currentSensed = 0;
abraham1 7:1726c40ad774 119 clock_t start, end, absoluteStart;
abraham1 12:6e13c34c4295 120 int ticks; // ticks counted on encoder
abraham1 12:6e13c34c4295 121 a=1; b=0; pwm.write(0); // this is forward. you can change a and b on a rig if it's turning the wrong way!
abraham1 12:6e13c34c4295 122 button.fall(&pressed); //adds pressed callback upon button push
abraham1 12:6e13c34c4295 123
abraham1 12:6e13c34c4295 124 /* we don't send all the information to matlab all the time. some collection and smoothing is done
abraham1 12:6e13c34c4295 125 here in order to not overload matlab with input making it slow. And to take a lot of data so we
abraham1 12:6e13c34c4295 126 can do smoothing quickly on this side */
abraham1 12:6e13c34c4295 127 double updatePeriod = 0.01; /* must select carefully. too fast and you don't get enough encoder ticks*/
abraham1 12:6e13c34c4295 128 double publishFrequency = 0.05; /* seconds. rate to publish to matlab. no need to overload matlab with input*/
abraham1 12:6e13c34c4295 129 double samplesPerPublish = (int)(publishFrequency/updatePeriod);
abraham1 0:dc5c88c2dd20 130 int publishCounter = 1;
abraham1 12:6e13c34c4295 131
abraham1 12:6e13c34c4295 132 double filterRatio = 0.1; // on the speed data
abraham1 12:6e13c34c4295 133 double currentFilterRatio = 0.035; // on the current data
abraham1 12:6e13c34c4295 134
abraham1 12:6e13c34c4295 135 /* the current sensor has some resting value. record and subtract that out */
abraham1 1:f97adef77f4b 136 float currentSensorOffset = 0; int i;
abraham1 4:f8a45966e63b 137 for(i=1;i<301;i++){ currentSensorOffset += currentSense.read(); }
abraham1 4:f8a45966e63b 138 currentSensorOffset = currentSensorOffset*VREF/300;
abraham1 1:f97adef77f4b 139 EncoderInitialise();
abraham1 1:f97adef77f4b 140 fflush(pc);
abraham1 1:f97adef77f4b 141
abraham1 11:e563823d3c94 142
abraham1 0:dc5c88c2dd20 143 while(1) {
abraham1 5:f4c237d0bb32 144
abraham1 10:ae139f40acc0 145
abraham1 12:6e13c34c4295 146 while(1) { // listen for pc go command
abraham1 7:1726c40ad774 147
abraham1 5:f4c237d0bb32 148 green = true;
abraham1 11:e563823d3c94 149 if(pc.readable()) {
abraham1 5:f4c237d0bb32 150 char charIn = pc.getc();
abraham1 6:73e417b1c521 151 if(charIn == 'g'){
abraham1 11:e563823d3c94 152 fflush(pc);
abraham1 7:1726c40ad774 153 absoluteStart = clock();
abraham1 11:e563823d3c94 154 end = clock();
abraham1 6:73e417b1c521 155 ZeroEncoder();
abraham1 6:73e417b1c521 156 velocity = 0;
abraham1 6:73e417b1c521 157 startcount = 0;
abraham1 6:73e417b1c521 158 endcount = 0;
abraham1 6:73e417b1c521 159 currentSensed = 0;
abraham1 12:6e13c34c4295 160 pwm.write(pwm_pulley);
abraham1 12:6e13c34c4295 161 break;
abraham1 12:6e13c34c4295 162 // note: it's really easy to change this if we want to include
abraham1 12:6e13c34c4295 163 // variable speed input from the command in the future.
abraham1 12:6e13c34c4295 164 // it's easiest to just pass chars as serial input when the
abraham1 12:6e13c34c4295 165 // mbed is listening (I find it's the most reliable)
abraham1 12:6e13c34c4295 166 // so an easy way would just be to pass a 1 2 3 ... 9 0
abraham1 12:6e13c34c4295 167 // corresponding to some percentage of the max output voltage
abraham1 12:6e13c34c4295 168 // as the start value for the run
abraham1 12:6e13c34c4295 169 // ask me for previous pulley interface mbed and matlab code
abraham1 12:6e13c34c4295 170 // where I have that implimented
abraham1 6:73e417b1c521 171 }
abraham1 3:df56bf381572 172 }
abraham1 11:e563823d3c94 173
abraham1 5:f4c237d0bb32 174 wait(0.05);
abraham1 7:1726c40ad774 175
abraham1 5:f4c237d0bb32 176 }
abraham1 5:f4c237d0bb32 177
abraham1 5:f4c237d0bb32 178
abraham1 12:6e13c34c4295 179 while(1) { //start collecting and publishing data
abraham1 7:1726c40ad774 180
abraham1 5:f4c237d0bb32 181 green = false;
abraham1 5:f4c237d0bb32 182 wait(updatePeriod);
abraham1 5:f4c237d0bb32 183 start = end;
abraham1 5:f4c237d0bb32 184 end = clock();
abraham1 5:f4c237d0bb32 185 time_between_readings = ((double)(end - start)) / CLOCKS_PER_SEC;
abraham1 5:f4c237d0bb32 186 startcount = endcount;
abraham1 5:f4c237d0bb32 187 endcount = GetCounts();
abraham1 5:f4c237d0bb32 188 ticks = endcount-startcount;
abraham1 5:f4c237d0bb32 189 if(abs(ticks)>CPR/2) /***** for rollover case: *****/
abraham1 5:f4c237d0bb32 190 { ticks = ((ticks<0)-(ticks>0))*(CPR-abs(ticks)); }
abraham1 12:6e13c34c4295 191
abraham1 5:f4c237d0bb32 192 velocity = filterRatio*((double)ticks)/CPR*2*PI/time_between_readings + (1-filterRatio)*velocity; /* with filtering*/
abraham1 5:f4c237d0bb32 193 currentSensed = currentFilterRatio*((double)currentSense.read()*VREF-currentSensorOffset) + (1-currentFilterRatio)*currentSensed;
abraham1 12:6e13c34c4295 194
abraham1 12:6e13c34c4295 195 if(pc.readable()) //pc will send r (reset) when run is finished
abraham1 0:dc5c88c2dd20 196 {
abraham1 5:f4c237d0bb32 197 char charIn = pc.getc();
abraham1 5:f4c237d0bb32 198 if(charIn == 'r'){
abraham1 11:e563823d3c94 199 fflush(pc);
abraham1 11:e563823d3c94 200 pwm.write(0.0);
abraham1 5:f4c237d0bb32 201 break;
abraham1 5:f4c237d0bb32 202 }
abraham1 5:f4c237d0bb32 203 }
abraham1 12:6e13c34c4295 204 if(publishCounter == samplesPerPublish) { //only publish onece every samplesPerPublish
abraham1 11:e563823d3c94 205 printf("%+9.5f,%+8.4f,%+9.4f,%+8.2f\n", currentSensed/currentSensorOutputRatio, pwm.read(), velocity, ((double)(end-absoluteStart)/CLOCKS_PER_SEC));
abraham1 12:6e13c34c4295 206 publishCounter = 1; //output formatting very important. running a simple string length checksum on matlab side to discard bad data
abraham1 7:1726c40ad774 207 }
abraham1 5:f4c237d0bb32 208 publishCounter++;
abraham1 5:f4c237d0bb32 209
abraham1 5:f4c237d0bb32 210 }
abraham1 5:f4c237d0bb32 211
abraham1 7:1726c40ad774 212
abraham1 0:dc5c88c2dd20 213 }
abraham1 0:dc5c88c2dd20 214
abraham1 0:dc5c88c2dd20 215 }
abraham1 0:dc5c88c2dd20 216
abraham1 0:dc5c88c2dd20 217
abraham1 0:dc5c88c2dd20 218
abraham1 0:dc5c88c2dd20 219
abraham1 0:dc5c88c2dd20 220
abraham1 0:dc5c88c2dd20 221
abraham1 0:dc5c88c2dd20 222
abraham1 0:dc5c88c2dd20 223