void ADDAC_Probabilistic::update(float _p1, float _p2, float _p3, float _p4, float _offSet){ // DO SOMETHING !
min1=0.0f;
max1=_p1+_offSet;
if (max1>1.0f) max1-=1.0f;
min2=max1;
max2=mapfloat(_p2, 0.0f, 1.0f, max1, 1.0f);
min3=max2;
max3=mapfloat(_p3, 0.0f, 1.0f, max2, 1.0f);
min4=max3;
max4=mapfloat(_p4, 0.0f, 1.0f, max3, 1.0f);
min5=max4;
max5=1.0f;
if(startUp){
calc();
startUp=false;
}
//update CVstream
if (glideOn) CVstream=glide();
else CVstream = randomResult;
CVstream = mapfloat(CVstream,0.0f,5.0f,0.0f,1.0f);
}
static void degradedUpdate(void) {
u32 now = millis();
//Turn off the engine, try to maintain zero pitch with an easy algorithm and try to maintain zero yawing rate
//First, the engine
outputCommandSet.timestamp = now;
outputCommandSet.thrust = 0;
//Now, pitch
if(now - currentAttitude.timestamp > SENSOR_TIMEOUT) {
outputCommandSet.pitch = 127; //Neutral pitch
} else {
float pitch = currentAttitude.pitch; //This is a +-90 range value
outputCommandSet.pitch = mapfloat(pitch, 20, -20, 0, 255); //We give full rudder at +-45
}
//Finally, yaw
if(now - (curGyro.timestamp / 1000) > SENSOR_TIMEOUT) { //gyro timestamps are in microseconds
outputCommandSet.yaw = 127;
} else {
float yawRate = curGyro.z; //In radians per second
outputCommandSet.yaw = mapfloat(yawRate, +MAX_ROTATION, -MAX_ROTATION, 0, 255);
}
}
float ADDAC_Probabilistic::glide() {
diffTime = abs(bangTimeOld - bangTime);
long inct=(long)constrain(millis()-diffTime,bangTimeOld,bangTime);
glideStream=mapfloat(inct,bangTimeOld,bangTime,randomResultOld,randomResult);
actualPosition=glideStream;
return glideStream;
}
float ADDAC_SuperFormula::getY(){
if(oldY<=y)oldY+=interpolationY;
if(oldY>=y)oldY-=interpolationY;
//map max and min values checked on processing if n1 value set to 50 //???
pY=mapfloat(oldY,minValueY,maxValueY,0.0f,1.0f);
pY=constrain(pY,0.0f,1.0f);
// Serial.print("OldY : ");
// Serial.print(oldY);
// Serial.print(" Y : ");
// Serial.print(y);
// Serial.print(" interpolationY : ");
// Serial.print(interpolationY,DEC);
return pY;
}
float ADDAC_SuperFormula::getX(){
if(oldX<=x)oldX+=interpolationX;
if(oldX>=x)oldX-=interpolationX;
//map max and min values checked on processing if n1 value set to 50 //???
pX = mapfloat(oldX, minValueX, maxValueX, 0.0f, 1.0f);
pX = constrain(pX,0.0f,1.0f);
// Serial.print("OldX : ");
// Serial.print(oldX);
// Serial.print(" X : ");
// Serial.print(x);
// Serial.print(" interpolationX : ");
// Serial.print(interpolationX);
return pX;
}
static void
matrix(void)
{
int count; /* pattern repeats this many times */
long total; /* expect this many patterns */
/*
*
* Reads a matrix from *fp_in, translates it into a PostScript gray scale image,
* and writes the result on stdout. For now only one matrix is allowed per input
* file. Matrix elements are floating point numbers arranged in row major order
* in the input file. In addition each input file may contain an optional header
* that defines special things like the dimension of the matrix, a window into
* the matrix that will be displayed, and an interval list.
*
* If we're reading from stdin we first make a copy in a temporary file so we can
* can properly position ourselves after we've looked for the header. Originally
* wasn't always making a copy of stdin, but I've added a few things to what's
* accepted in the header and this simplifies the job. An alternative would be
* to always require a header and mark the end of it by some string. Didn't like
* that approach much - may fix things up later.
*
*/
if ( fp_in == stdin ) /* make a copy so we can seek etc. */
copystdin();
rows = dfltrows; /* new dimensions for the next matrix */
columns = dfltcols;
buildilist(interval); /* build the default ilist[] */
addcolormap(colormap); /* add the colormap - if not NULL */
setwindow(window); /* and setup the initial matrix window */
nmstat = dostats; /* want statistics? */
getheader(); /* matrix dimensions at the very least */
dimensions(); /* make sure we have the dimensions etc. */
patcount = 0;
total = rows * columns;
eptr = rptr + (wlist[2] - wlist[0] + 1);
redirect(++page);
fprintf(fp_out, "%s %d %d\n", PAGE, page, printed+1);
fprintf(fp_out, "save\n");
writerequest(printed+1, fp_out);
fprintf(fp_out, "%d %d bitmap\n", wlist[2] - wlist[0] + 1, wlist[3] - wlist[1] + 1);
while ( patcount != total && fscanf(fp_in, "%f", &element) != EOF ) {
if ( inwindow() ) *rptr++ = mapfloat(element);
if ( ++patcount % columns == 0 )
if ( inrange() )
putrow();
} /* End while */
if ( total != patcount )
error(FATAL, "matrix format error");
labelmatrix();
if ( fp_out == stdout ) printed++;
fprintf(fp_out, "showpage\n");
fprintf(fp_out, "restore\n");
fprintf(fp_out, "%s %d %d\n", ENDPAGE, page, printed);
} /* End of matrix */
// newValue - value to smooth, smoothFactor - smooth intensity 0-1.
void ADDAC_SuperFormula::update(float m, float n2, float n3, long _speed) {
long oldTime=millis();
m*=1000.0f;
m*2;
n2*=100.0f;
n3*=100.0f;
//values checked on processing for default min and max values//
n2=constrain(n2,0,100);
n3=constrain(n3,0,100);
n2=mapfloat(n2,0,100,-50,100);
n3=mapfloat(n3,0,100,-50,100);
///////////////////////////////////////////////////////////////
float phi = TWO_PI / 360;
if(startUp){
metroTime=1;
startUp=false;
}
else metroTime=_speed;
if(metro.set(metroTime)){
if (mirror) {
if(pos>numPoints){
pos=numPoints;
inc=-1;
}
if(pos<loopMin){
pos=loopMin;
inc=1;
}
pos+=inc;
}
else{
pos++;
if(pos>numPoints) pos = loopMin;
}
oldPos=pos;
superformulaPoint(m, n2, n3, phi);
}
long timeCpu=fabs(actualTime-oldTime);
cpuPos++;
if(cpuPos>9) cpuPos=0;
cpuTime[cpuPos]=timeCpu;
actualTime=millis();
}
