2.007 PulleyInterface mbed code. Biomimetics robotics lab. Sangbae Kim. Ben Katz. For use with PulleyInterface.mlapp
Diff: main.cpp
- Revision:
- 3:df56bf381572
- Parent:
- 2:a7100c183940
- Child:
- 4:f8a45966e63b
diff -r a7100c183940 -r df56bf381572 main.cpp --- a/main.cpp Fri Feb 03 20:52:05 2017 +0000 +++ b/main.cpp Sat Feb 04 16:33:45 2017 +0000 @@ -2,22 +2,20 @@ #include "mbed.h" #include "time.h" +#include "stdio.h" +#include "ctype.h" InterruptIn button(USER_BUTTON); PwmOut pwm(D5);//do not use D3 DigitalOut a(D2); DigitalOut b(D4); -int endcount, startcount; -double time_between_readings; -double velocity; +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); -int cpr = 900; // Encoder counts per revolution. Change to match your encoder - -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 motorCurrent(A5); //hook up to Vout on current sensor -Serial pc(USBTX, USBRX); -DigitalOut green(LED2); +const int cpr = 900; // Encoder counts per revolution. Change to match your encoder void EncoderInitialise(void) { @@ -60,16 +58,16 @@ int pwmV = (int)(100*pwm_float); printf("pwm2: %d\n\r",pwmV); if(pwmV == 0){ - pwm.write(0.1); - } else if (pwmV == 10){ + pwm.write(0.05); + } else if (pwmV == 5){ pwm.write(0.2); } else if (pwmV == 20){ - pwm.write(0.5); - } else if (pwmV == 50){ - pwm.write(1.0); - } else if (pwmV == 100){ + pwm.write(0.75); + } else if (pwmV == 75){ pwm.write(0.0); - } + } else { + pwm.write(0.0); + } } @@ -78,6 +76,10 @@ const double vRef = 3; + int endcount, startcount; + double time_between_readings; + double velocity; + double currentSensed = 0; clock_t start; clock_t end = clock(); int ticks; @@ -88,8 +90,9 @@ double samplesPerPublish = (int)(publishFrequency/updatePeriod); /*this improves time response of filter and maintains smoothness*/ int publishCounter = 1; double filterRatio = 0.05; + double currentFilterRatio = 0.02; float currentSensorOffset = 0; int i; - for(i=1;i<101;i++){ currentSensorOffset += motorCurrent.read(); } + for(i=1;i<101;i++){ currentSensorOffset += currentSense.read(); } currentSensorOffset = currentSensorOffset*vRef/100; EncoderInitialise(); @@ -108,16 +111,24 @@ 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*/ + + currentSensed = currentFilterRatio*((double)currentSense.read()*vRef-currentSensorOffset) + (1-currentFilterRatio)*currentSensed; + if(pc.readable()) + { + char charIn = pc.getc(); + if(isdigit(charIn)){ + double abrahamsCommand = (double)(charIn - '0'); + pwm.write(abrahamsCommand/10.0); + } + } + + if(publishCounter == samplesPerPublish) { - char abrahamsCommand = pc.getc(); - if(abrahamsCommand == '1'){ green = 1; } - if(abrahamsCommand == '0'){ green = 0; } +// printf("%f,%f,%f\n", (double)currentSense.read()*vRef-currentSensorOffset, motorVoltage.read(), velocity); + printf("%f,%f,%f\n", currentSensed, motorVoltage.read(), velocity); + publishCounter = 1; } - if(publishCounter == samplesPerPublish){ - printf("%f,%f,%f\n", (double)motorCurrent.read()*vRef-currentSensorOffset, motorVoltage.read(), velocity); - publishCounter = 1; - } publishCounter++; }