bobbycar/other/filtertest/filtertest.pde

200 lines
6.0 KiB
Plaintext

import controlP5.*;
ControlP5 cp5;
int myColor = color(0,0,0);
long lastReadADC=0;
long lastSend=0;
long lastHBLoop=0;
int ADC_CALIB_THROTTLE_MIN=2000; //controller
int ADC_CALIB_THROTTLE_MAX=3120; //controller
int ADC_READTIME=10; //controller. in ms
int adc_throttle_raw=ADC_CALIB_THROTTLE_MIN; //controller. raw value read from adc (here slider)
float adc_throttle=adc_throttle_raw; //controller. internally filtered value
float ADC_THROTTLE_FILTER=0.05; //controller
int SENDPERIOD=50; //controller. in ms
int sendSpeed=0; //transmittet speed value over usart
int LOOPTIME=5; //hoverboard.
int RATE=30; //hoverboard. rate value, see config.h. default 30
float FILTER=0.1; //hoverboard. filter value, see config.h default 0.1
float speedRate=0; //hoverboard.implemented as fixdt. what comes out of rate limiting.
int outSpeed=0; //hoverboard. speedvalue for pwm. filtered speedRate
Slider SinputValue;
Slider SfilteredInputValue;
Slider SspeedRate;
Slider SoutSpeed;
int inputValue = ADC_CALIB_THROTTLE_MIN;
int maxvalue=1000;
int timetestMode=0;
long timeteststarted_time=0;
int outSpeedThresholdIncrease=int(1000*0.707); //threshold for timing stop for increasing speed
int outSpeedThresholdDecrease=int(1000*(1-0.707)); //threshold for timing stop for decreasing speed
long timingtest_start=0; //for timing testing
long timingtest_end=0; //for timing testing
void setup() {
frameRate(int(1000.0/5));
size(700,400);
noStroke();
cp5 = new ControlP5(this);
// add a horizontal sliders, the value of this slider will be linked
// to variable 'sliderValue'
SinputValue=cp5.addSlider("inputValue")
.setPosition(10,50)
.setSize(500,30)
.setRange(ADC_CALIB_THROTTLE_MIN,ADC_CALIB_THROTTLE_MAX)
;
SfilteredInputValue=cp5.addSlider("filteredInputValue")
.setPosition(10,50+30+10)
.setSize(500,20)
.setRange(ADC_CALIB_THROTTLE_MIN,ADC_CALIB_THROTTLE_MAX)
;
SspeedRate=cp5.addSlider("speedRate")
.setPosition(10,50+30+10+20+10)
.setSize(500,20)
.setRange(0,1000)
;
SoutSpeed=cp5.addSlider("outSpeed")
.setPosition(1,50+30+10+20+10+20+10)
.setSize(500,30)
.setRange(0,1000)
;
}
void draw() {
long loopmillis=millis();
//Time testing
switch (timetestMode) {
case 0: //started
println("Starting Timing Test 'Increase'. Startvalue="+ADC_CALIB_THROTTLE_MIN);
SinputValue.setValue(ADC_CALIB_THROTTLE_MIN); //reset values to minimum
SfilteredInputValue.setValue(ADC_CALIB_THROTTLE_MIN);
SspeedRate.setValue(0);
SoutSpeed.setValue(0);
timetestMode++;
timeteststarted_time=loopmillis;
break;
case 1: //wait some time
if (loopmillis-timeteststarted_time>1000){
println("Set input to value="+ADC_CALIB_THROTTLE_MAX);
timetestMode++;
SinputValue.setValue(ADC_CALIB_THROTTLE_MAX); //Jump value
timingtest_start=loopmillis;
}
break;
case 2: //wait until threshold reached
if (outSpeed>=outSpeedThresholdIncrease){
println("Threshold "+outSpeed+" (outSpeed) reached");
timetestMode++;
timingtest_end=loopmillis;
println("Increase Time="+(timingtest_end-timingtest_start)+"ms");
timeteststarted_time=loopmillis;
}
break;
case 3: //wait some time in between test
if (loopmillis-timeteststarted_time>1000){
println("Waiting between tests");
timetestMode++;
timingtest_start=loopmillis;
}
break;
case 4://started
println("Starting Timing Test 'Decrease'. Startvalue="+ADC_CALIB_THROTTLE_MAX);
SinputValue.setValue(ADC_CALIB_THROTTLE_MAX); //reset values to maximum
SfilteredInputValue.setValue(ADC_CALIB_THROTTLE_MAX);
SspeedRate.setValue(1000);
SoutSpeed.setValue(1000);
timetestMode++;
break;
case 5: //wait some time
if (loopmillis-timeteststarted_time>1000){
println("Set input to value="+ADC_CALIB_THROTTLE_MIN);
timetestMode++;
SinputValue.setValue(ADC_CALIB_THROTTLE_MIN); //Jump value
timingtest_start=loopmillis;
}
break;
case 6: //wait until threshold reached
if (outSpeed<=outSpeedThresholdDecrease){
println("Threshold "+outSpeed+" (outSpeed) reached");
timetestMode++;
timingtest_end=loopmillis;
println("Decrease Time="+(timingtest_end-timingtest_start)+"ms");
}
break;
}
if (loopmillis-lastReadADC>ADC_READTIME){ //frequency of controller adc read/filtering
lastReadADC=millis();
adc_throttle_raw = inputValue; //adc_throttle_raw = analogRead(PIN_THROTTLE);
adc_throttle = adc_throttle*(1-ADC_THROTTLE_FILTER) + adc_throttle_raw*ADC_THROTTLE_FILTER; //adc_throttle = adc_throttle*(1-ADC_THROTTLE_FILTER) + adc_throttle_raw*ADC_THROTTLE_FILTER;
SfilteredInputValue.setValue(adc_throttle);
}
if (loopmillis-lastSend>SENDPERIOD){ //Frequency of transmitting new values to the hoverboard
lastSend=millis();
sendSpeed=int(constrain( map(adc_throttle, ADC_CALIB_THROTTLE_MIN, ADC_CALIB_THROTTLE_MAX, 0, 1000 ) ,0, 1000));
}
if (loopmillis-lastHBLoop>LOOPTIME){ //frequency of hoverboard mainboard (looptime)
lastHBLoop=millis();
int cmd=sendSpeed; //commanded value
//rateLimiter16 function
float q0; //int16_t q0;
float q1; //int16_t q1;
q0 = (cmd-speedRate); //q0 = (u << 4) - *y;
if (q0 > RATE) { //if (q0 > rate) {
q0=RATE; // q0 = rate;
}else{ //} else {
q1=-RATE; // q1 = -rate;
if (q0<q1) {// if (q0 < q1) {
q0=q1;// q0 = q1;
}// }
} //}
speedRate = q0 + speedRate; //*y = q0 + *y;
SspeedRate.setValue(speedRate);
outSpeed= int(outSpeed*(1-FILTER) + speedRate*FILTER); //filtLowPass16 function
SoutSpeed.setValue(outSpeed);
}
//fill(inputValue*1.0/maxvalue*255);
//rect(0,0,width,100);
}
/*
void slider(float theColor) {
myColor = color(theColor);
println("a slider event. setting background to "+theColor);
}
*/