Joystick.cpp

00001 #include "Joystick.h"
00002 
00003 AnalogIn hin(p19);
00004 AnalogIn vin(p18);
00005 
00006 joyhv last = {0.5,0.5};
00007 
00008 double scaleb(double min, double max, double bmin, double bmax, double num){
00009     double shifted = num + (bmin - min);    //shifted bmin down to min
00010     return (shifted - min) * (bmax-bmin)/(max-min) + min; //scaled bmax to max
00011 }
00012 
00013 double doscale(double pos){
00014     double min, max, midmin, midmax, scaled;
00015     
00016     min = 0.087;
00017     max = 0.98;
00018     
00019     midmin = 0.49;
00020     midmax = 0.57;
00021 
00022     
00023     if(pos < midmin){           // lower than 0.5 tolerance - backwards
00024         scaled = scaleb(min, midmin, 0, 0.5, pos);
00025     } else if (pos > midmax){   //greater than 0.5 tolerance - forwards
00026         scaled = scaleb(midmax, max, 0.5, 1, pos);
00027     } else {
00028         scaled = 0.5;
00029     }
00030     
00031     //check if out of bounds [0,1]
00032     
00033     if(scaled < 0)
00034         scaled = 0;
00035     
00036     if(scaled > 1)
00037         scaled = 1;
00038     
00039     return scaled;
00040 }
00041 
00042 
00043 double dofilter(double last, double num, double factor){
00044 
00045     return last*factor + num*(1-factor);
00046 
00047 }
00048 
00049 Joystick::Joystick(PinName b, PinName h, PinName v) : _b(b), _h(h), _v(v) {
00050 }
00051 
00052 Joystick::operator joyhv (){
00053     joyhv data;
00054     data.v = _v.read();
00055     data.h = _h.read();
00056     return data;
00057 }
00058 
00059 double Joystick::getV (){
00060     return _v.read();
00061 }
00062 
00063 double Joystick::getH (){
00064     return _h.read();
00065 }
00066 
00067 void Joystick::rise (void (*fptr)(void)){ 
00068     _b.rise(fptr);
00069 }
00070 
00071 void Joystick::fall (void (*fptr)(void)){
00072     _b.fall(fptr);
00073 }
00074 
00075 //simple filtering
00076 joyhv Joystick::filter(joyhv read, double factor){
00077 
00078     joyhv filtered = {0.5,0.5};
00079     
00080     filtered.v = dofilter(last.v, read.v, factor);
00081     filtered.h = dofilter(last.h, read.h, factor);
00082     
00083     last.v = filtered.v;
00084     last.h = filtered.h;
00085     
00086     return filtered;
00087 }
00088 
00089 joyhv Joystick::scale(joyhv read){
00090     
00091     joyhv scaled = {0.5,0.5};
00092     
00093     scaled.v = doscale(read.v);
00094     scaled.h = doscale(read.h);
00095     
00096     return scaled;
00097     
00098 }