2.007 PulleyInterface mbed code. Biomimetics robotics lab. Sangbae Kim. Ben Katz. For use with PulleyInterface.mlapp
main.cpp
- Committer:
- abraham1
- Date:
- 2017-04-15
- Revision:
- 12:6e13c34c4295
- Parent:
- 11:e563823d3c94
- Child:
- 13:d9d0eb9103d3
File content as of revision 12:6e13c34c4295:
/*** Basic Program Flow: mbed turns on. initializes. enters main while loop then first sub while loop where it listens for a go command mbed recieves go commmand, initializes for data collection, breaks out of first sub while enters second sub while. starts collecting and publishing data, listening for stop mbed hears stop, breaks out of second while loop, returns back to first while loop Keep in Mind: Serial communication between mbed matlab sometimes sucks, and often breaks down Most of the things in this program (and the corresponding matlab one) are meant to minimize the amount of bad data that gets passed over serial and maintain the connection. ***/ #include "mbed.h" #include "time.h" #include "stdio.h" #include "ctype.h" #define PI 3.14159265358979323846 //Just for basic debugging //User button controls motor speed //Green LED should turn on while listening to pc for input before starting run InterruptIn button(USER_BUTTON); DigitalOut green(LED2); // Pololu VNH5019 Motor Driver Carrier. https://www.pololu.com/product/1451 PwmOut pwm(D5); //pwm input on motor controller. do not use D3 DigitalOut a(D2); //IN_A input on motor controller DigitalOut b(D4); //IN_B input on motor controller //Hook up to Vout on current sensor //SparkFun Hall-Effect Current Sensor Breakout - ACS712 //https://www.sparkfun.com/products/8882 AnalogIn currentSense(A5); //For communication with pc through matlab //Make sure baud rates are equal Serial pc(USBTX, USBRX, 115200); const int CPR = 900*4; // Encoder counts per revolution (900). Change to match your encoder. x4 for quadrature const double VREF = 3; // Microcontroller reference voltage const float currentSensorOutputRatio = 0.185; // Volts/Amp specified by current sensor. Divide Voltage by cSenseOutput to get current const float PSupply_Voltage = 12.0; // Voltage input from powersupply const float Output_Voltage = 6.0; // Maximum output voltage desired const float pwm_pulley = Output_Voltage/PSupply_Voltage; ///setup code for encoder on ***** pins PA0 and PA1 (A0 and A1) ****** /// void EncoderInitialise(void) { // configure GPIO PA0 & PA1 as inputs for Encoder RCC->AHB1ENR |= 0x00000001; // Enable clock for GPIOA GPIOA->MODER |= GPIO_MODER_MODER0_1 | GPIO_MODER_MODER1_1 ; //PA0 & PA1 as Alternate Function /*!< GPIO port mode register, Address offset: 0x00 */ GPIOA->OTYPER |= GPIO_OTYPER_OT_0 | GPIO_OTYPER_OT_1 ; //PA0 & PA1 as Inputs /*!< GPIO port output type register, Address offset: 0x04 */ GPIOA->OSPEEDR |= 0x00000011;//|= GPIO_OSPEEDER_OSPEEDR0 | GPIO_OSPEEDER_OSPEEDR1 ; // Low speed /*!< GPIO port output speed register, Address offset: 0x08 */ GPIOA->PUPDR |= GPIO_PUPDR_PUPDR0_1 | GPIO_PUPDR_PUPDR1_1 ; // Pull Down /*!< GPIO port pull-up/pull-down register, Address offset: 0x0C */ GPIOA->AFR[0] |= 0x00000011 ; // AF01 for PA0 & PA1 /*!< GPIO alternate function registers, Address offset: 0x20-0x24 */ GPIOA->AFR[1] |= 0x00000000 ; // /*!< GPIO alternate function registers, Address offset: 0x20-0x24 */ // configure TIM2 as Encoder input RCC->APB1ENR |= 0x00000001; // Enable clock for TIM2 TIM2->CR1 = 0x0001; // CEN(Counter Enable)='1' < TIM control register 1 TIM2->SMCR = 0x0003; // SMS='011' (Encoder mode 3) < TIM slave mode control register TIM2->CCMR1 = 0x0101; // CC1S='01' CC2S='01' < TIM capture/compare mode register 1 TIM2->CCMR2 = 0x0000; // < TIM capture/compare mode register 2 TIM2->CCER = 0x0011; // CC1P CC2P < TIM capture/compare enable register TIM2->PSC = 0x0000; // Prescaler = (0+1) < TIM prescaler TIM2->ARR = CPR; // reload at CPR < TIM auto-reload register TIM2->CNT = 0x0000; //reset the counter before we use it } //Zero encoder count// void ZeroEncoder() { TIM2->CNT=0 ; //reset timer count to zero } int GetCounts(void) { int count = TIM2->CNT; //Read the timer count register return count; } void pressed() { float pwm_float = pwm.read(); int pwmV = (int)(100*pwm_float); if(pwmV == 0){ pwm.write(0.05); } else if (pwmV == 5){ pwm.write(0.2); } else if (pwmV == 20){ pwm.write(0.75); } else if (pwmV == 75){ pwm.write(0.0); } else { pwm.write(0.0); } } int main() { int endcount, startcount; // encoder counts double time_between_readings; // between encoder readings double velocity; // radians/time double currentSensed = 0; clock_t start, end, absoluteStart; int ticks; // ticks counted on encoder 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! button.fall(&pressed); //adds pressed callback upon button push /* we don't send all the information to matlab all the time. some collection and smoothing is done here in order to not overload matlab with input making it slow. And to take a lot of data so we can do smoothing quickly on this side */ double updatePeriod = 0.01; /* must select carefully. too fast and you don't get enough encoder ticks*/ double publishFrequency = 0.05; /* seconds. rate to publish to matlab. no need to overload matlab with input*/ double samplesPerPublish = (int)(publishFrequency/updatePeriod); int publishCounter = 1; double filterRatio = 0.1; // on the speed data double currentFilterRatio = 0.035; // on the current data /* the current sensor has some resting value. record and subtract that out */ float currentSensorOffset = 0; int i; for(i=1;i<301;i++){ currentSensorOffset += currentSense.read(); } currentSensorOffset = currentSensorOffset*VREF/300; EncoderInitialise(); fflush(pc); while(1) { while(1) { // listen for pc go command green = true; if(pc.readable()) { char charIn = pc.getc(); if(charIn == 'g'){ fflush(pc); absoluteStart = clock(); end = clock(); ZeroEncoder(); velocity = 0; startcount = 0; endcount = 0; currentSensed = 0; pwm.write(pwm_pulley); break; // note: it's really easy to change this if we want to include // variable speed input from the command in the future. // it's easiest to just pass chars as serial input when the // mbed is listening (I find it's the most reliable) // so an easy way would just be to pass a 1 2 3 ... 9 0 // corresponding to some percentage of the max output voltage // as the start value for the run // ask me for previous pulley interface mbed and matlab code // where I have that implimented } } wait(0.05); } while(1) { //start collecting and publishing data green = false; wait(updatePeriod); start = end; end = clock(); time_between_readings = ((double)(end - start)) / CLOCKS_PER_SEC; startcount = endcount; endcount = GetCounts(); ticks = endcount-startcount; if(abs(ticks)>CPR/2) /***** for rollover case: *****/ { ticks = ((ticks<0)-(ticks>0))*(CPR-abs(ticks)); } velocity = filterRatio*((double)ticks)/CPR*2*PI/time_between_readings + (1-filterRatio)*velocity; /* with filtering*/ currentSensed = currentFilterRatio*((double)currentSense.read()*VREF-currentSensorOffset) + (1-currentFilterRatio)*currentSensed; if(pc.readable()) //pc will send r (reset) when run is finished { char charIn = pc.getc(); if(charIn == 'r'){ fflush(pc); pwm.write(0.0); break; } } if(publishCounter == samplesPerPublish) { //only publish onece every samplesPerPublish printf("%+9.5f,%+8.4f,%+9.4f,%+8.2f\n", currentSensed/currentSensorOutputRatio, pwm.read(), velocity, ((double)(end-absoluteStart)/CLOCKS_PER_SEC)); publishCounter = 1; //output formatting very important. running a simple string length checksum on matlab side to discard bad data } publishCounter++; } } }