124 lines
3.4 KiB
Plaintext
124 lines
3.4 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=0; //controller. raw value read from adc (here slider)
|
||
|
|
float adc_throttle=0; //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;
|
||
|
|
|
||
|
|
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(10,50+30+10+20+10+20+10)
|
||
|
|
.setSize(500,30)
|
||
|
|
.setRange(0,1000)
|
||
|
|
;
|
||
|
|
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
void draw() {
|
||
|
|
long loopmillis=millis();
|
||
|
|
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);
|
||
|
|
}
|
||
|
|
|
||
|
|
*/
|