From 3bc8898039e8fa89129c653a57d58c00943edd99 Mon Sep 17 00:00:00 2001
From: Fisch <interfisch@gmx.net>
Date: Sun, 21 Mar 2021 15:31:05 +0100
Subject: [PATCH] copy hoverbrettvisualizer for osd starting point

---
 logdata_visualization/Visualization.pde       | 419 ++++++++++++++++++
 .../logdata_visualization.pde                 | 164 +++++++
 2 files changed, 583 insertions(+)
 create mode 100644 logdata_visualization/Visualization.pde
 create mode 100644 logdata_visualization/logdata_visualization.pde

diff --git a/logdata_visualization/Visualization.pde b/logdata_visualization/Visualization.pde
new file mode 100644
index 0000000..d31caac
--- /dev/null
+++ b/logdata_visualization/Visualization.pde
@@ -0,0 +1,419 @@
+
+abstract class Visualization
+{
+  float value=0;
+  float value2=1;
+  
+  PVector posOrigin = new PVector(100,100);
+  float valueMin=0;
+  float valueMax=100;
+  float value2Min=0; //value 2 for twodimensional visualization
+  float value2Max=1;
+  
+  float valueMinRecord = Float.NaN; //nan means no value here
+  float valueMaxRecord = Float.NaN;
+  float value2MinRecord = Float.NaN; //nan means no value here
+  float value2MaxRecord = Float.NaN;
+  boolean showMinMax=false;
+  
+  //default colors (not all used by every implementation)
+  color cmain = color(0,0,0);
+  color cscale = color(100,100,100);
+  color cborder = color(204,104,0);
+  color cmin = color(0,150,0);
+  color cmax = color(150,0,0);
+  color ctext = color(0,0,0);
+  
+  int textsize=12;
+  float textWidthScale=1.0/2*this.textsize/2; //*text.length()*
+  int showdecimals=2;
+  
+  String title="";
+  
+  
+  public abstract void drawVis();
+  public void setValue(float pv){
+    //this.value=constrain(pv,valueMin,valueMax);
+    this.value=pv;
+    if (this.showMinMax){
+      if (Float.isNaN(this.valueMinRecord) || this.value<this.valueMinRecord){
+        this.valueMinRecord=this.value;        
+      }
+      if (Float.isNaN(this.valueMaxRecord) || this.value>this.valueMaxRecord){
+        this.valueMaxRecord=this.value;        
+      }
+    }
+  }
+  
+  public void setValue2(float pv){
+    //this.value2=constrain(pv,valueMin,valueMax);
+    this.value2=pv;
+    if (this.showMinMax){
+      if (Float.isNaN(this.value2MinRecord) || this.value2<this.value2MinRecord){
+        this.value2MinRecord=this.value2;        
+      }
+      if (Float.isNaN(this.value2MaxRecord) || this.value2>this.value2MaxRecord){
+        this.value2MaxRecord=this.value2;        
+      }
+    }
+  }
+  
+  public float getValueNormalized() {
+    return (constrain(this.value,this.valueMin,this.valueMax)-this.valueMin)/(this.valueMax-this.valueMin);
+  }
+  public float getValueMinNormalized() {
+    return (constrain(this.valueMinRecord,this.valueMin,this.valueMax)-this.valueMin)/(this.valueMax-this.valueMin);
+  }
+  public float getValueMaxNormalized() {
+    return (constrain(this.valueMaxRecord,this.valueMin,this.valueMax)-this.valueMin)/(this.valueMax-this.valueMin);
+  }
+  
+  public float getValue2Normalized() {
+    return (constrain(this.value2,this.value2Min,this.value2Max)-this.value2Min)/(this.value2Max-this.value2Min);
+  }
+  public float getValue2MinNormalized() {
+    return (constrain(this.value2MinRecord,this.value2Min,this.value2Max)-this.value2Min)/(this.value2Max-this.value2Min);
+  }
+  public float getValue2MaxNormalized() {
+    return (constrain(this.value2MaxRecord,this.value2Min,this.value2Max)-this.value2Min)/(this.value2Max-this.value2Min);
+  }
+  
+  public void setShowMinMax(boolean pshowMinMax){
+    this.showMinMax = pshowMinMax;
+  }
+  
+  public void setcmain(color pc){
+    this.cmain=pc;
+  }
+  public void setcscale(color pc){
+    this.cscale=pc;
+  }
+  public void setcborder(color pc){
+    this.cborder=pc;
+  }
+  public void setcmin(color pc){
+    this.cmin=pc;
+  }
+  public void setcmax(color pc){
+    this.cmax=pc;
+  }
+  public void setctext(color pc){
+    this.ctext=pc;
+  }
+  
+  public void setTitle(String pt) {
+    this.title=pt;
+  }
+  
+  public void setshowdecimals(int pd) {
+    this.showdecimals=pd;
+  }
+  
+  public String getFormattedValue(double pv){
+    if( ( (int)(pv*100)/100.0 )%1==0){
+      return ""+(int)pv;
+    }else{ //has decimals
+      return String.format("%."+this.showdecimals+"f", pv); //limit decimal places
+    }
+  }
+  
+}
+
+public class BarV extends Visualization {
+  PVector size = new PVector(10,100);
+  
+  public BarV(int px, int py, int pw, int ph, float pvmin, float pvmax) {
+    super.valueMin=pvmin;
+    super.valueMax=pvmax;
+    super.posOrigin= new PVector(px,py); //lower left corner
+    this.size = new PVector(pw,ph); //to the right and up
+  }
+  
+  public void drawVis() {
+    rectMode(CORNERS);
+    textSize(super.textsize);
+    
+    fill(super.cmain); noStroke();
+    rect(super.posOrigin.x,super.posOrigin.y,super.posOrigin.x+this.size.x,super.posOrigin.y-( this.size.y*super.getValueNormalized()) );
+    
+    
+    if (!Float.isNaN(super.valueMinRecord)){
+      stroke(super.cmin);
+      line(super.posOrigin.x,super.posOrigin.y-( this.size.y*super.getValueMinNormalized()) ,super.posOrigin.x+this.size.x,super.posOrigin.y-( this.size.y*super.getValueMinNormalized()) );
+    }
+    if (!Float.isNaN(super.valueMaxRecord)){
+      stroke(super.cmax);
+      line(super.posOrigin.x, super.posOrigin.y-( this.size.y*super.getValueMaxNormalized()),  super.posOrigin.x+this.size.x,super.posOrigin.y-( this.size.y*super.getValueMaxNormalized()) );
+    }
+    
+    noFill(); stroke(this.cborder);
+    rect(super.posOrigin.x,super.posOrigin.y,super.posOrigin.x+this.size.x,super.posOrigin.y- this.size.y );
+    
+    //text
+    fill(super.ctext);
+    text(super.getFormattedValue(super.valueMin),super.posOrigin.x+this.size.x+1,super.posOrigin.y+super.textsize/2);
+    text(super.getFormattedValue(super.valueMax),super.posOrigin.x+this.size.x+1,super.posOrigin.y-this.size.y+super.textsize/2);
+    text(super.getFormattedValue(super.value),super.posOrigin.x+this.size.x+1,super.posOrigin.y-this.size.y/2+super.textsize/2);
+    
+    //Title
+    text(super.title, super.posOrigin.x-super.title.length()*super.textWidthScale, super.posOrigin.y+super.textsize*1.5);
+    
+  }
+}
+
+public class BarH extends Visualization {
+  PVector size = new PVector(10,100);
+  
+  public BarH(int px, int py, int pw, int ph,float pvmin, float pvmax) {
+    super.valueMin=pvmin;
+    super.valueMax=pvmax;
+    super.posOrigin= new PVector(px,py); //lower left corner
+    this.size = new PVector(pw,ph); //to the right and up
+  }
+  
+  public void drawVis() {
+    rectMode(CORNERS);
+    textSize(super.textsize);
+    
+    fill(super.cmain); noStroke();
+    rect(super.posOrigin.x,super.posOrigin.y,super.posOrigin.x+( this.size.x*super.getValueNormalized()),super.posOrigin.y- this.size.y );
+    
+    if (!Float.isNaN(super.valueMinRecord)){
+      stroke(super.cmin);
+      line(super.posOrigin.x+( this.size.x*super.getValueMinNormalized()),super.posOrigin.y,super.posOrigin.x+( this.size.x*super.getValueMinNormalized()),super.posOrigin.y- this.size.y );
+    }
+    if (!Float.isNaN(super.valueMaxRecord)){
+      stroke(super.cmax);
+      line(super.posOrigin.x+( this.size.x*super.getValueMaxNormalized()),super.posOrigin.y,super.posOrigin.x+( this.size.x*super.getValueMaxNormalized()),super.posOrigin.y- this.size.y );
+    }
+    
+    noFill(); stroke(super.cborder);
+    rect(super.posOrigin.x,super.posOrigin.y,super.posOrigin.x+this.size.x,super.posOrigin.y- this.size.y );
+    
+    //text
+    fill(super.ctext);
+    text(super.getFormattedValue(super.valueMin),super.posOrigin.x-super.getFormattedValue(super.valueMin).length()*super.textWidthScale,super.posOrigin.y-this.size.y-1);
+    text(super.getFormattedValue(super.valueMax),super.posOrigin.x+this.size.x-super.getFormattedValue(super.valueMax).length()*super.textWidthScale,super.posOrigin.y-this.size.y-1);
+    text(super.getFormattedValue(super.value),super.posOrigin.x+this.size.x/2-super.getFormattedValue(super.value).length()*super.textWidthScale,super.posOrigin.y-this.size.y-1);
+    
+    
+    //Title
+    text(super.title, super.posOrigin.x+this.size.x/2-super.title.length()*super.textWidthScale,super.posOrigin.y+this.size.y+1);
+    
+  }
+}
+
+public class Tacho extends Visualization {
+  int size;
+  
+  
+  public Tacho(int px, int py, int psize, float pvmin, float pvmax) {
+    super.valueMin=pvmin;
+    super.valueMax=pvmax;
+    super.posOrigin= new PVector(px,py); //circle center
+    this.size = psize; //radius from the center
+  }
+  
+  public void drawVis() {
+    rectMode(CORNERS);
+    textSize(super.textsize);
+    
+    stroke(super.cmain);
+    float angle=PI-super.getValueNormalized()*PI; //0=right, positive=CCW
+    line(super.posOrigin.x,super.posOrigin.y,super.posOrigin.x+cos(angle)*this.size*0.8 ,super.posOrigin.y-sin(angle)*this.size*0.8); //draw tacho needle
+    
+    if (!Float.isNaN(super.valueMinRecord)){
+      stroke(this.cmin);
+      float angleMin=PI-super.getValueMinNormalized()*PI; //0=right, positive=CCW
+      line(super.posOrigin.x+cos(angleMin)*this.size*0.65 ,super.posOrigin.y-sin(angleMin)*this.size*0.65,super.posOrigin.x+cos(angleMin)*this.size*0.75 ,super.posOrigin.y-sin(angleMin)*this.size*0.75); //draw tacho needle min
+    }
+    if (!Float.isNaN(super.valueMaxRecord)){
+      stroke(this.cmax);
+      float angleMax=PI-super.getValueMaxNormalized()*PI; //0=right, positive=CCW
+      line(super.posOrigin.x+cos(angleMax)*this.size*0.65 ,super.posOrigin.y-sin(angleMax)*this.size*0.65,super.posOrigin.x+cos(angleMax)*this.size*0.75 ,super.posOrigin.y-sin(angleMax)*this.size*0.75); //draw tacho needle max
+    }
+    
+    stroke(this.cscale);
+    fill(super.ctext);
+    float _steps=0.1;
+    for (float i=0;i<=1+_steps; i+=_steps){
+       float a=PI-PI*i;
+       line(super.posOrigin.x+cos(a)*this.size*0.85 ,super.posOrigin.y-sin(a)*this.size*0.85 ,super.posOrigin.x+cos(a)*this.size ,super.posOrigin.y-sin(a)*this.size);
+       String _text=super.getFormattedValue(super.valueMin+(super.valueMax-super.valueMin)*i);
+       text(_text,super.posOrigin.x+cos(a)*this.size*1.1-_text.length()*super.textWidthScale-1 ,super.posOrigin.y-sin(a)*this.size*1.1+super.textsize/2-1);
+    }
+    
+    //text   
+    text(super.getFormattedValue(super.value),super.posOrigin.x-super.getFormattedValue(super.value).length()*super.textWidthScale,super.posOrigin.y+super.textsize/2-this.size*0.3);
+    
+    //Title
+    text(super.title, super.posOrigin.x-super.title.length()*super.textWidthScale, super.posOrigin.y+super.textsize*1.5);
+    
+  }
+}
+
+
+public class Direction extends Visualization {
+  int size;
+  float angleoffset;
+  float minvisiblevalue;
+  float maxvisiblevalue;
+  
+  public Direction(int px, int py, int psize,float pvmin, float pvmax, float pvlmin, float pvlmax, float pangleoffset) {
+    super.valueMin=pvmin;
+    super.valueMax=pvmax;
+    super.posOrigin= new PVector(px,py); //center
+    this.size = psize; //radius from the center
+    
+    super.value2Min=pvlmin;
+    super.value2Max=pvlmax;
+    
+    this.angleoffset=pangleoffset;
+    
+    this.minvisiblevalue=valueMin;
+    this.maxvisiblevalue=valueMax;
+  }
+  
+  public Direction(int px, int py, int psize,float pvmin, float pvmax, float pvlmin, float pvlmax, float pangleoffset, float pminvisiblevalue, float pmaxvisiblevalue) {
+    super.valueMin=pvmin;
+    super.valueMax=pvmax;
+    super.posOrigin= new PVector(px,py); //center
+    this.size = psize; //radius from the center
+    
+    super.value2Min=pvlmin;
+    super.value2Max=pvlmax;
+    
+    this.angleoffset=pangleoffset;
+    
+    this.minvisiblevalue=pminvisiblevalue;
+    this.maxvisiblevalue=pmaxvisiblevalue;
+  }
+  
+  
+  
+  public void drawVis() {
+    rectMode(CORNERS);
+    textSize(super.textsize);
+    
+    stroke(super.cborder); noFill();
+    if (this.minvisiblevalue==super.valueMin & this.maxvisiblevalue==super.valueMax) {
+      ellipseMode(RADIUS);  //centerx, centery, width,height for ellipse
+      ellipse(super.posOrigin.x, super.posOrigin.y, this.size,this.size);
+    }else{
+      arc(super.posOrigin.x, super.posOrigin.y, this.size*2,this.size*2, this.angleoffset+2*PI -this.minvisiblevalue/super.valueMin*PI, 2*PI+this.angleoffset +this.maxvisiblevalue/super.valueMax*PI, PIE);
+    }
+    
+    stroke(super.cmain);
+    float angle=map(super.getValueNormalized(),0,1,0,2*PI)+this.angleoffset;
+    //float _vecsize=this.size*( (super.value2-super.value2Min)/(super.value2Max-super.value2Min));
+    float _vecsize=this.size*super.getValue2Normalized();
+    line(super.posOrigin.x,super.posOrigin.y,super.posOrigin.x+cos(angle)*_vecsize,super.posOrigin.y-sin(angle)*_vecsize);
+    
+    line(super.posOrigin.x+cos(angle-0.1)*_vecsize*0.9,super.posOrigin.y-sin(angle-0.1)*_vecsize*0.9,super.posOrigin.x+cos(angle)*_vecsize,super.posOrigin.y-sin(angle)*_vecsize); //arrow
+    line(super.posOrigin.x+cos(angle+0.1)*_vecsize*0.9,super.posOrigin.y-sin(angle+0.1)*_vecsize*0.9,super.posOrigin.x+cos(angle)*_vecsize,super.posOrigin.y-sin(angle)*_vecsize);
+    
+    //text
+    fill(super.ctext);
+    text(super.getFormattedValue(super.value),super.posOrigin.x-super.getFormattedValue(super.value).length()*super.textWidthScale,super.posOrigin.y+super.textsize*1.5*1);
+    if (super.value2<super.value2Max){ //display only if in use
+      text("l="+super.getFormattedValue(super.value2),super.posOrigin.x-("l="+super.getFormattedValue(super.value2)).length()*super.textWidthScale,super.posOrigin.y+super.textsize*1.5*2);
+    }
+    
+    //Title
+    text(super.title, super.posOrigin.x-super.title.length()*super.textWidthScale, super.posOrigin.y+super.textsize*1.5*3);
+  }
+}
+
+
+
+
+public class GraphRoll extends Visualization {
+  PVector size;
+  float[] valuearray;
+  float[] valueMinarray;
+  float[] valueMaxarray;
+  int arraypos;
+  int recordevery=1; //minimum value =1, the higher the slower
+  int recordevery_counter=0;
+  
+  public GraphRoll(int px, int py, int psx, int psy,float pvmin, float pvmax,int precordevery) {
+    super.valueMin=pvmin;
+    super.valueMax=pvmax;
+    super.posOrigin= new PVector(px,py); //center
+    this.size = new PVector(psx,psy);
+    
+    this.recordevery= precordevery;
+    
+    this.valuearray = new float[psx]; //array size equals window width
+    this.arraypos=0; //points to position to write to next (current value is at arraypos-1)
+    this.valueMinarray = new float[psx];
+    this.valueMaxarray = new float[psx];
+  }
+  
+  
+  
+  public void drawVis() {
+    //update history
+    this.recordevery_counter++;
+    if (this.recordevery_counter>=this.recordevery){
+      this.valuearray[this.arraypos]=super.getValueNormalized();
+      this.arraypos++;
+      this.arraypos%=this.valuearray.length;
+      this.recordevery_counter=0;
+      
+      if (super.showMinMax) {
+        this.valueMinarray[this.arraypos]=super.getValueMinNormalized();
+        this.valueMaxarray[this.arraypos]=super.getValueMaxNormalized();
+        super.valueMinRecord=Float.NaN;
+        super.valueMaxRecord=Float.NaN;
+      }
+    }
+    
+    
+    rectMode(CORNERS);
+    textSize(super.textsize);
+    
+    stroke(super.cborder);
+    noFill();
+    rect(super.posOrigin.x,super.posOrigin.y,super.posOrigin.x+this.size.x,super.posOrigin.y-this.size.y);
+    
+     noFill();
+    int _cpos=this.arraypos;
+    int _x=0; //position of _ya
+    while (_cpos!=((this.arraypos-1+this.valuearray.length)%this.valuearray.length)) { //go trough all values starting at oldest value
+      float _ya=this.valuearray[_cpos];
+      float _yb=this.valuearray[(_cpos+1)%this.valuearray.length];
+      
+      //float _yaMin=this.valueMinarray[_cpos];
+      float _ybMin=this.valueMinarray[(_cpos+1)%this.valuearray.length];
+      float _ybMax=this.valueMaxarray[(_cpos+1)%this.valuearray.length];
+      
+      
+      if (super.showMinMax) {
+        stroke(super.cmin);
+        //line(super.posOrigin.x+_x,super.posOrigin.y-_yaMin*this.size.y, super.posOrigin.x+_x+1,super.posOrigin.y-_ybMin*this.size.y);
+        line(super.posOrigin.x+_x+1,super.posOrigin.y-_ybMin*this.size.y, super.posOrigin.x+_x+1,super.posOrigin.y-_yb*this.size.y);
+        stroke(super.cmax);
+        line(super.posOrigin.x+_x+1,super.posOrigin.y-_yb*this.size.y, super.posOrigin.x+_x+1,super.posOrigin.y-_ybMax*this.size.y);
+      }
+      
+      stroke(super.cmain);
+      line(super.posOrigin.x+_x,super.posOrigin.y-_ya*this.size.y, super.posOrigin.x+_x+1,super.posOrigin.y-_yb*this.size.y);
+      
+      
+      
+      _x++;
+      _cpos++;
+      _cpos%=this.valuearray.length;
+    }
+    
+    //text
+    fill(super.ctext);
+    text(super.getFormattedValue(super.value), super.posOrigin.x+this.size.x-50, super.posOrigin.y+super.textsize*1.5);
+    text(super.getFormattedValue(super.valueMin), super.posOrigin.x+this.size.x+1, super.posOrigin.y+super.textsize*0.5-1);
+    text(super.getFormattedValue(super.valueMax), super.posOrigin.x+this.size.x+1, super.posOrigin.y-this.size.y+super.textsize*0.5-1);
+    //Title
+    text(super.title, super.posOrigin.x, super.posOrigin.y+super.textsize*1.5);
+  }
+}
+  
diff --git a/logdata_visualization/logdata_visualization.pde b/logdata_visualization/logdata_visualization.pde
new file mode 100644
index 0000000..8cd0430
--- /dev/null
+++ b/logdata_visualization/logdata_visualization.pde
@@ -0,0 +1,164 @@
+import processing.serial.*;
+
+Serial serialport;
+Visualization visVoltage;
+Visualization visCurrent;
+Visualization visSpeedl;
+Visualization visSpeedr;
+Visualization visYaw;
+Visualization visGT;
+Visualization visGT_Vertical;
+
+int speedl=0;
+int speedr=0;
+int booleanvalues=0;
+float voltage = 50;
+float current = 0.0;
+float yaw=0;
+int gt_length=0;
+int gt_horizontal=0;
+int gt_vertical=0;
+
+long lastReceive=0; //last time serial received
+long lastDelay=0;
+
+
+void setup() {
+  size(640, 450);
+  frameRate(100);
+  
+  printArray(Serial.list());
+  serialport = new Serial(this, Serial.list()[32], 115200);
+  
+  
+  visVoltage = new BarV(50,150,10,100,10*3.3,12*4.2);
+  visVoltage.setcmain(color(100,100,100));
+  //vis = new BarH(150,150,100,10,0,100);
+  //visVoltage = new Tacho(150,150,100,0,100);
+  //vis = new Direction(150,150,100,0,100,0,1,0);
+  
+  visVoltage.setShowMinMax(true);
+  visVoltage.setTitle("Voltage [V]");
+  
+  visCurrent= new Tacho(150+100,150,100,0,100);
+  visCurrent.setShowMinMax(true);
+  visCurrent.setTitle("Current [A]");
+  
+  visSpeedl = new BarV(20+80,120,10,100,-1000,1000);
+  visSpeedl.setTitle("SpeedL");
+  
+  visSpeedr = new BarV(20,120,10,100,-1000,1000);
+  visSpeedr.setTitle("SpeedR");
+  
+  visYaw = new Direction(150+100,120,100,0,360,0,1,0);
+  visYaw.setTitle("Yaw");
+  
+  visGT = new Direction(150+100+220,120,100,-127/30*180,127/30*180,0,2500,PI/2+PI);
+  visGT.setTitle("Gametrak");
+  visGT_Vertical = new BarV(150+100+220+110,120+50,10,100,-127,127);
+  visGT_Vertical.setTitle("Vertical");
+  
+}
+
+void draw() {
+  receive();
+  
+  background(255);
+  visVoltage.setValue(voltage);
+  visCurrent.setValue(current);
+  visSpeedl.setValue(speedl);
+  visSpeedr.setValue(speedr);
+  visYaw.setValue(yaw);
+  visGT.setValue(-gt_horizontal);
+  visGT.setValue2(gt_length);
+  visGT_Vertical.setValue(gt_vertical);
+  
+  
+  //visVoltage.drawVis();
+  //visCurrent.drawVis();
+  visSpeedl.drawVis();
+  visSpeedr.drawVis();
+  visYaw.drawVis();
+  visGT.drawVis();
+  visGT_Vertical.drawVis();
+  
+  fill(color(0,0,0));
+  textSize(12);
+  long _delay=lastDelay;
+  if (millis()-lastReceive>1000){ //show counting up if update too long ago
+    _delay=millis()-lastReceive;
+  }
+  text("Delay="+(_delay), 5,12);
+}
+
+
+public void receive()
+{
+  boolean received=false;
+  byte[] inBuffer = new byte[17];
+  while(serialport.available()>0) {
+    
+    inBuffer = serialport.readBytes();
+    serialport.readBytes(inBuffer);
+    if (inBuffer != null && inBuffer.length==17) {
+      received=true;
+      int _address=0;
+      speedl = extract_int16_t(inBuffer,_address); _address+=2;
+      speedr = extract_int16_t(inBuffer,_address); _address+=2;
+      booleanvalues = extract_uint8_t(inBuffer,_address); _address+=1;
+      voltage = extract_float(inBuffer, _address);  _address+=4;
+      //current = extract_float(inBuffer,_address);_address+=4;
+      yaw = extract_float(inBuffer,_address);_address+=4;
+      gt_length = extract_uint16_t(inBuffer,_address); _address+=2;
+      gt_horizontal = extract_int8_t(inBuffer,_address); _address+=1;
+      gt_vertical = extract_int8_t(inBuffer,_address); _address+=1;
+      //println("yaw="+yaw);
+      //println("gt_horizontal="+gt_horizontal);
+      
+      boolean motorenabled=boolean(booleanvalues & (1<<0)>>0); //check bit 0
+      int controlmode=(booleanvalues & (3<<1))>>1; //check bit 1 and 2
+     
+      println("motorenabled="+motorenabled+" controlmode="+controlmode);
+    }
+    
+  }
+  if (received){
+    lastDelay=millis()-lastReceive;
+    lastReceive=millis();
+  }
+}
+  
+public int extract_int8_t(byte array[], int startbyte) {
+  if ( ((int)array[startbyte] & 0x80) == 0x00 ) { //2's complement, not negative
+    return ((int)array[startbyte] & 0xff);
+  }else{
+    return -128 + ( (int)array[startbyte] & 0x7f);
+  }
+}
+
+public int extract_uint8_t(byte array[], int startbyte) {
+  return ((int)array[startbyte] & 0xff);
+}
+
+public int extract_uint16_t(byte array[], int startbyte) {
+  return ((int)array[startbyte] & 0xff) | ((int)array[startbyte+1] & 0xff)<<8;
+}
+
+public int extract_int16_t(byte array[], int startbyte) {
+  if ( ((int)array[startbyte+1] & 0x80) == 0x00 ) { //2's complement, not negative
+    return ( ((int)array[startbyte] & 0xff) | ((int)array[startbyte+1] & 0x7f)<<8 );
+  }else{ //value is negative
+    return -32768 + ( ((int)array[startbyte] & 0xff) | ((int)array[startbyte+1] & 0x7f)<<8 );
+  }
+}
+
+public float extract_float(byte array[], int startbyte) {
+    int MASK = 0xff;
+    int bits = 0;
+    int i = startbyte+3; //+3 because goes backwards and has 4 bytes
+    for (int shifter = 3; shifter >= 0; shifter--) {
+    bits |= ((int) array[i] & MASK) << (shifter * 8);
+    i--;
+    }
+    return Float.intBitsToFloat(bits);
+}  
\ No newline at end of file