For flywheel rig. Interfaces with matlab flywheel interface. 2.007 MIT Biomimetics Robotics Lab. Sangbae Kim. Ben Katz.
Diff: main.cpp
- Revision:
- 4:f8a45966e63b
- Parent:
- 3:df56bf381572
- Child:
- 5:f4c237d0bb32
diff -r df56bf381572 -r f8a45966e63b main.cpp --- a/main.cpp Sat Feb 04 16:33:45 2017 +0000 +++ b/main.cpp Sat Feb 04 21:19:06 2017 +0000 @@ -5,18 +5,20 @@ #include "stdio.h" #include "ctype.h" +#define PI 3.14159265358979323846 + InterruptIn button(USER_BUTTON); PwmOut pwm(D5);//do not use D3 DigitalOut a(D2); DigitalOut b(D4); -AnalogIn motorVoltage(A4); //hook up to high voltage motor terminal with capacitor. this doesn't share a common ground though hmm... can I hook up battery ground AnalogIn currentSense(A5); //hook up to Vout on current sensor Serial pc(USBTX, USBRX, 115200); DigitalOut green(LED2); -const int cpr = 900; // Encoder counts per revolution. Change to match your encoder - +const int CPR = 900; // Encoder counts per revolution. Change to match your encoder +const double VREF = 3; //Microcontroller reference voltage +const float currentSensorOutputRatio = 0.185; // Volts/Amp. Divide Voltage by cSenseOutput to get current void EncoderInitialise(void) { // configure GPIO PA0 & PA1 as inputs for Encoder @@ -38,7 +40,7 @@ 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->ARR = CPR; // reload at CPR < TIM auto-reload register TIM2->CNT = 0x0000; //reset the counter before we use it } @@ -56,7 +58,6 @@ void pressed() { float pwm_float = pwm.read(); int pwmV = (int)(100*pwm_float); - printf("pwm2: %d\n\r",pwmV); if(pwmV == 0){ pwm.write(0.05); } else if (pwmV == 5){ @@ -74,8 +75,6 @@ int main() { - - const double vRef = 3; int endcount, startcount; double time_between_readings; double velocity; @@ -86,18 +85,19 @@ a=1; b=0; pwm.write(0); button.fall(&pressed); double updatePeriod = 0.01; /* must select carefully */ - double publishFrequency = 0.1; /* seconds. rate to publish to matlab */ + double publishFrequency = 0.05; /* seconds. rate to publish to matlab */ double samplesPerPublish = (int)(publishFrequency/updatePeriod); /*this improves time response of filter and maintains smoothness*/ int publishCounter = 1; - double filterRatio = 0.05; + double filterRatio = 0.045; double currentFilterRatio = 0.02; float currentSensorOffset = 0; int i; - for(i=1;i<101;i++){ currentSensorOffset += currentSense.read(); } - currentSensorOffset = currentSensorOffset*vRef/100; + for(i=1;i<301;i++){ currentSensorOffset += currentSense.read(); } + currentSensorOffset = currentSensorOffset*VREF/300; EncoderInitialise(); fflush(pc); + /*** wait here for matlab information like voltage before starting? ***/ while(1) { @@ -108,16 +108,19 @@ 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/time_between_readings + (1-filterRatio)*velocity; /* with filtering*/ + 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; + currentSensed = currentFilterRatio*((double)currentSense.read()*VREF-currentSensorOffset) + (1-currentFilterRatio)*currentSensed; if(pc.readable()) { char charIn = pc.getc(); - if(isdigit(charIn)){ + if(charIn == 'r'){ + fflush(pc); + green = !green; + } else if(isdigit(charIn)) { double abrahamsCommand = (double)(charIn - '0'); pwm.write(abrahamsCommand/10.0); } @@ -125,8 +128,7 @@ if(publishCounter == samplesPerPublish) { -// printf("%f,%f,%f\n", (double)currentSense.read()*vRef-currentSensorOffset, motorVoltage.read(), velocity); - printf("%f,%f,%f\n", currentSensed, motorVoltage.read(), velocity); + printf("%f,%f,%f\n", currentSensed/currentSensorOutputRatio, pwm.read(), velocity); publishCounter = 1; } publishCounter++;