quadcopter cufe
/
FollowWall2
Go to goal robot with follow wall algo
SpaceSensor.cpp
- Committer:
- khaledelmadawi
- Date:
- 2014-04-08
- Revision:
- 0:efef62b55c86
File content as of revision 0:efef62b55c86:
#include "mbed.h" #include "SpaceSensor.h" SpaceSensor::SpaceSensor(PinName p_tx, PinName p_rx): _SpaceSensor(p_tx, p_rx) { begin(115200); st = new char[128]; vecf= new float[15]; } /**********************************************************************************************************************/ /*4.3.1 Commands for Reading Filtered Sensor Data*/ /**********************************************************************************************************************/ void SpaceSensor::ReadfilteredtaredQuaternion(float *Quaternion){ _SpaceSensor.puts(":0\n"); _SpaceSensor.scanf("%s", st); read_string(Quaternion,st); st[0]='\n'; } void SpaceSensor::ReadfilteredtaredEulerAngles(float *EulerAngles){ _SpaceSensor.puts(":1\n"); _SpaceSensor.scanf("%s", st); read_string(EulerAngles,st); st[0]='\n'; } void SpaceSensor::ReadfilteredtaredRotationMatrix(float *RotationMatrix){ _SpaceSensor.puts(":2\n"); _SpaceSensor.scanf("%s", st); read_string(RotationMatrix,st); st[0]='\n'; } void SpaceSensor::ReadfilteredtaredAxisAngle(float *Axis,float *Angle){ _SpaceSensor.puts(":3\n"); _SpaceSensor.scanf("%s", st); read_string(vecf,st); for(int i=0;i<3;i++){Axis[i]=vecf[i];} Angle[0]=vecf[3]; st[0]='\n'; } void SpaceSensor::ReadfilteredtaredTwoVector(float *Forward,float *Down){ _SpaceSensor.puts(":4\n"); _SpaceSensor.scanf("%s", st); read_string(vecf,st); for(int i=0;i<3;i++){Forward[i]=vecf[i];} for(int i=0;i<3;i++){Down[i]=vecf[i+3];} st[0]='\n'; } void SpaceSensor::Readfilteredgyrorates(float *Quaternion){ _SpaceSensor.puts(":5\n"); _SpaceSensor.scanf("%s", st); read_string(Quaternion,st); st[0]='\n'; } void SpaceSensor::ReadfiltereduntaredQuaternion(float *Quaternion){ _SpaceSensor.puts(":6\n"); _SpaceSensor.scanf("%s", st); read_string(Quaternion,st); st[0]='\n'; } void SpaceSensor::ReadfiltereduntaredEulerAngles(float *EulerAngles){ _SpaceSensor.puts(":7\n"); _SpaceSensor.scanf("%s", st); read_string(EulerAngles,st); st[0]='\n'; } void SpaceSensor::ReadfiltereduntaredRotationMatrix(float *RotationMatrix){ _SpaceSensor.puts(":8\n"); _SpaceSensor.scanf("%s", st); read_string(RotationMatrix,st); st[0]='\n'; } void SpaceSensor::ReadfiltereduntaredAxisAngle(float *Axis,float *Angle){ _SpaceSensor.puts(":9\n"); _SpaceSensor.scanf("%s", st); read_string(vecf,st); for(int i=0;i<3;i++){Axis[i]=vecf[i];} Angle[0]=vecf[3]; st[0]='\n'; } void SpaceSensor::ReadfiltereduntaredTwoVector(float *Forward,float *Down){ _SpaceSensor.puts(":10\n"); _SpaceSensor.scanf("%s", st); read_string(vecf,st); for(int i=0;i<3;i++){Forward[i]=vecf[i];} for(int i=0;i<3;i++){Down[i]=vecf[i+3];} st[0]='\n'; } void SpaceSensor::ReadfilteredtaredForwardandDownVectors(float *Forward,float *Down){ _SpaceSensor.puts(":11\n"); _SpaceSensor.scanf("%s", st); read_string(vecf,st); for(int i=0;i<3;i++){Forward[i]=vecf[i];} for(int i=0;i<3;i++){Down[i]=vecf[i+3];} st[0]='\n'; } void SpaceSensor::ReadfilteredNorthEarthVectors(float *North,float *Earth){ _SpaceSensor.puts(":12\n"); _SpaceSensor.scanf("%s", st); read_string(vecf,st); for(int i=0;i<3;i++){North[i]=vecf[i];} for(int i=0;i<3;i++){Earth[i]=vecf[i+3];} st[0]='\n'; } /**********************************************************************************************************************/ /*4.3.2 Commands for Interfacing with Electronic Systems*/ /**********************************************************************************************************************/ void SpaceSensor::Setinterrupttype(int mode){ sprintf( st ,":29,%d\n",mode); _SpaceSensor.puts(st); } void SpaceSensor::Readinterrupttype(int *interrupttype){ _SpaceSensor.puts(":30\n"); read_string(vecf,st); interrupttype[0]=(int)vecf[0]; interrupttype[1]=(int)vecf[1]; } void SpaceSensor::Readinterruptstatus(int *interruptstatus){ _SpaceSensor.puts(":31\n"); read_string(vecf,st); interruptstatus[0]=(int)vecf[0]; } /**********************************************************************************************************************/ /*4.3.3 Commands for Reading Normalized Sensor Data*/ /**********************************************************************************************************************/ void SpaceSensor::Readall(float *gyro,float *accelerometer,float *compass){ _SpaceSensor.puts(":32\n"); _SpaceSensor.scanf("%s", st); read_string(vecf,st); for(int i=0;i<3;i++){gyro[i]=vecf[i];} for(int i=0;i<3;i++){accelerometer[i]=vecf[i+3];} for(int i=0;i<3;i++){compass[i]=vecf[i+6];} st[0]='\n'; } void SpaceSensor::Readgyros(float *gyro){ _SpaceSensor.puts(":33\n"); _SpaceSensor.scanf("%s", st); read_string(gyro,st); st[0]='\n'; } void SpaceSensor::Readaccelerometer(float *accelerometer){ _SpaceSensor.puts(":34\n"); _SpaceSensor.scanf("%s", st); read_string(accelerometer,st); st[0]='\n'; } void SpaceSensor::Readcompass(float *compass){ _SpaceSensor.puts(":35\n"); _SpaceSensor.scanf("%s", st); read_string(compass,st); st[0]='\n'; } void SpaceSensor::ReadtemperatureC(float *temp){ _SpaceSensor.puts(":36\n"); _SpaceSensor.scanf("%s", st); read_string(temp,st); st[0]='\n'; } void SpaceSensor::ReadtemperatureF(float *temp){ _SpaceSensor.puts(":37\n"); _SpaceSensor.scanf("%s", st); read_string(temp,st); st[0]='\n'; } void SpaceSensor::Readconfidencefactor(float *confidencefactor){ _SpaceSensor.puts(":38\n"); _SpaceSensor.scanf("%s", st); read_string(confidencefactor,st); st[0]='\n'; } void SpaceSensor::Readaccelerometerunnormalized(float *accelunnormalized){ _SpaceSensor.puts(":39\n"); _SpaceSensor.scanf("%s", st); read_string(accelunnormalized,st); st[0]='\n'; } void SpaceSensor::Readcompassunnormalized(float *compassunnormalized){ _SpaceSensor.puts(":30\n"); _SpaceSensor.scanf("%s", st); read_string(compassunnormalized,st); st[0]='\n'; } /**********************************************************************************************************************/ /*4.3.4 Commands for Reading Raw Sensor Data*/ /**********************************************************************************************************************/ void SpaceSensor::Readallraw(float *gyro,float *accelerometer,float *compass){ _SpaceSensor.puts(":64\n"); _SpaceSensor.scanf("%s", st); read_string(vecf,st); for(int i=0;i<3;i++){gyro[i]=vecf[i];} for(int i=0;i<3;i++){accelerometer[i]=vecf[i+3];} for(int i=0;i<3;i++){compass[i]=vecf[i+6];} st[0]='\n'; } void SpaceSensor::Readgyroraw(float *gyro){ _SpaceSensor.puts(":65\n"); _SpaceSensor.scanf("%s", st); read_string(gyro,st); st[0]='\n'; } void SpaceSensor::Readaccelerometerraw(float *accelerometer){ _SpaceSensor.puts(":66\n"); _SpaceSensor.scanf("%s", st); read_string(accelerometer,st); st[0]='\n'; } void SpaceSensor::Readcompassraw(float *compass){ _SpaceSensor.puts(":67\n"); _SpaceSensor.scanf("%s", st); read_string(compass,st); st[0]='\n'; } /**********************************************************************************************************************/ /*4.3.5 Commands for Setting Filter Parameters*/ /**********************************************************************************************************************/ void SpaceSensor::Tarewithcurrentorientation(){ _SpaceSensor.puts(":96\n"); } void SpaceSensor::Tarewithquaternion(float Quaternion[4]){ sprintf( st ,":97,%f,%f,%f,%f\n",Quaternion[0],Quaternion[1],Quaternion[2],Quaternion[3]); _SpaceSensor.puts(st); } void SpaceSensor::Tarewithrotationmatrix(float RotationMatrix[9]){ sprintf( st ,":98,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",RotationMatrix[0],RotationMatrix[1],RotationMatrix[2],RotationMatrix[3],RotationMatrix[4],RotationMatrix[5],RotationMatrix[6],RotationMatrix[7],RotationMatrix[8]); _SpaceSensor.puts(st); } void SpaceSensor::SetStaticRhoModeAccelerometer(float rho){ sprintf( st ,":99,%f\n",rho); _SpaceSensor.puts(st); } void SpaceSensor::SetConfidenceRhoModeAccelerometer(float min,float max){ sprintf( st ,":100,%f,%f\n",min,max); _SpaceSensor.puts(st); } void SpaceSensor::SetStaticRhoModeCompass(float rho){ sprintf( st ,":101,%f\n",rho); _SpaceSensor.puts(st); } void SpaceSensor::SetConfidenceRhoModeCompass(float min, float max){ sprintf( st ,":102,%f,%f\n",min,max); _SpaceSensor.puts(st); } void SpaceSensor::SetDesiredUpdateRate(int t){ sprintf( st ,":103,%d\n",t); _SpaceSensor.puts(st); } void SpaceSensor::SetMultiReferenceVectorsWithCurrentOrientation(){ _SpaceSensor.puts(":l04\n"); } void SpaceSensor::SetReferenceVectorMode(int mode){ sprintf( st ,":105,%d\n",mode); _SpaceSensor.puts(st); } void SpaceSensor::SetOversampleRate(int rate){ sprintf( st ,":106,%d\n",rate); _SpaceSensor.puts(st); } void SpaceSensor::EnableDisablegyros(int mode){ sprintf( st ,":107,%d\n",mode); _SpaceSensor.puts(st); } void SpaceSensor::EnableDisableAccelerometer(int mode){ sprintf( st ,":108,%d\n",mode); _SpaceSensor.puts(st); } void SpaceSensor::EnableDisableCompass(int mode){ sprintf( st ,":109,%d\n",mode); _SpaceSensor.puts(st); } void SpaceSensor::ResetMultiReferenceVectorsToZero(){ _SpaceSensor.puts(":110\n"); } void SpaceSensor::SetMultiReferenceResolution(int mode){ sprintf( st ,":111,%d\n",mode); _SpaceSensor.puts(st); } void SpaceSensor::SetCompassMultiReferenceVector(int Index,float Vector[3]){ sprintf( st ,":112,%d,%f,%f,%f\n",Index,Vector[0],Vector[1],Vector[2]); _SpaceSensor.puts(st); } void SpaceSensor::SetCompassMultiReferenceCheckVector(int Index,float Vector[3]){ sprintf( st ,":113,%d,%f,%f,%f\n",Index,Vector[0],Vector[1],Vector[2]); _SpaceSensor.puts(st); } void SpaceSensor::SetAccelMultiReferenceVector(int Index,float Vector[3]){ sprintf( st ,":114,%d,%f,%f,%f\n",Index,Vector[0],Vector[1],Vector[2]); _SpaceSensor.puts(st); } void SpaceSensor::SetAccelMultiReferenceCheckVector(int Index,float Vector[3]){ sprintf( st ,":115,%d,%f,%f,%f\n",Index,Vector[0],Vector[1],Vector[2]); _SpaceSensor.puts(st); } void SpaceSensor::SetAxisDirections(int direction){ sprintf( st ,":116,%d\n",direction); _SpaceSensor.puts(st); } void SpaceSensor::SetRunningAveragePercent(float Percent){ sprintf( st ,":117,%f\n",Percent); _SpaceSensor.puts(st); } void SpaceSensor::SetCompassReferenceVector(float Vector[3]){ sprintf( st ,":118,%f,%f,%f\n",Vector[0],Vector[1],Vector[2]); _SpaceSensor.puts(st); } void SpaceSensor::SetAccelerometerReferenceVector(float Vector[3]){ sprintf( st ,":119,%f,%f,%f\n",Vector[0],Vector[1],Vector[2]); _SpaceSensor.puts(st); } void SpaceSensor::ResetKalmanFilter(){ _SpaceSensor.puts(":120\n"); } void SpaceSensor::SetAccelerometerRange(int Accelrange){ sprintf( st ,":121,%d\n",Accelrange); _SpaceSensor.puts(st); } void SpaceSensor::SetMultiReferenceWeightPower(float WeightPower){ sprintf( st ,":122,%f\n",WeightPower); _SpaceSensor.puts(st); } void SpaceSensor::EnableDisableFilter(int Mode){ sprintf( st ,":123,%d\n",Mode); _SpaceSensor.puts(st); } void SpaceSensor::SetRunningAverageMode(int Mode){ sprintf( st ,":124,%d\n",Mode); _SpaceSensor.puts(st); } void SpaceSensor::SetGyroscopeRange(int mode){ sprintf( st ,":125,%d\n",mode); _SpaceSensor.puts(st); } void SpaceSensor::SetCompassRange(int mode){ sprintf( st ,":126,%d\n",mode); _SpaceSensor.puts(st); } /**********************************************************************************************************************/ /*4.3.6 Commands for Reading Filter Parameters*/ /**********************************************************************************************************************/ void SpaceSensor::ReadTareOrientationQuaternion(float *Quaternion){ _SpaceSensor.puts(":128\n"); _SpaceSensor.scanf("%s", st); read_string(Quaternion,st); st[0]='\n'; } void SpaceSensor::ReadTareOrientationRotationMatrix(float *RotationMatrix){ _SpaceSensor.puts(":129\n"); _SpaceSensor.scanf("%s", st); read_string(RotationMatrix,st); st[0]='\n'; } void SpaceSensor::ReadRhoDataAccelerometer(int *Rhomode,float *minroh,float *maxroh){ _SpaceSensor.puts(":130\n"); _SpaceSensor.scanf("%s", st); read_string(vecf,st); Rhomode[0]=(int)vecf[0]; minroh[0]=vecf[1]; maxroh[0]=vecf[2]; st[0]='\n'; } void SpaceSensor::ReadRhoDataCompass(int *Rhomode,float *minroh,float *maxroh){ _SpaceSensor.puts(":131\n"); _SpaceSensor.scanf("%s", st); read_string(vecf,st); Rhomode[0]=(int)vecf[0]; minroh[0]=vecf[1]; maxroh[0]=vecf[2]; st[0]='\n'; } void SpaceSensor::ReadCurrentUpdateRate(int *rate){ _SpaceSensor.puts(":132\n"); _SpaceSensor.scanf("%s", st); read_string(vecf,st); rate[0]=(int)vecf[0]; st[0]='\n'; } void SpaceSensor::ReadCompassReferenceVector(float *Vector){ _SpaceSensor.puts(":133\n"); _SpaceSensor.scanf("%s", st); read_string(Vector,st); st[0]='\n'; } void SpaceSensor::ReadAccelerometerReferenceVector(float *Vector){ _SpaceSensor.puts(":134\n"); _SpaceSensor.scanf("%s", st); read_string(Vector,st); st[0]='\n'; } void SpaceSensor::ReadReferenceVectorMode(int *Mode){ _SpaceSensor.puts(":135\n"); _SpaceSensor.scanf("%s", st); read_string(vecf,st); Mode[0]=(int)vecf[0]; st[0]='\n'; } void SpaceSensor::ReadCompassMultiReferenceVector(int Index,float *Vector){ sprintf( st ,":136,%d\n",Index); _SpaceSensor.puts(st); _SpaceSensor.scanf("%s", st); read_string(Vector,st); st[0]='\n'; } void SpaceSensor::ReadCompassMultiReferenceCheckVector(int Index,float *Vector){ sprintf( st ,":137,%d\n",Index); _SpaceSensor.puts(st); _SpaceSensor.scanf("%s", st); read_string(Vector,st); st[0]='\n'; } void SpaceSensor::ReadAccelMultiReferenceVector(int Index,float *Vector){ sprintf( st ,":138,%d\n",Index); _SpaceSensor.puts(st); _SpaceSensor.scanf("%s", st); read_string(Vector,st); st[0]='\n'; } void SpaceSensor::ReadAccelMultiReferenceCheckVector(int Index,float *Vector){ sprintf( st ,":139,%d\n",Index); _SpaceSensor.puts(st); _SpaceSensor.scanf("%s", st); read_string(Vector,st); st[0]='\n'; } void SpaceSensor::ReadGyroEnabledState(int *Mode){ _SpaceSensor.puts(":140\n"); _SpaceSensor.scanf("%s", st); read_string(vecf,st); Mode[0]=(int)vecf[0]; st[0]='\n'; } void SpaceSensor::ReadAccelerometerEnabledState(int *Mode){ _SpaceSensor.puts(":141\n"); _SpaceSensor.scanf("%s", st); read_string(vecf,st); Mode[0]=(int)vecf[0]; st[0]='\n'; } void SpaceSensor::ReadCompassEnabledState(int *Mode){ _SpaceSensor.puts(":142\n"); _SpaceSensor.scanf("%s", st); read_string(vecf,st); Mode[0]=(int)vecf[0]; st[0]='\n'; } void SpaceSensor::ReadAxisDirections(int *direction){ _SpaceSensor.puts(":143\n"); _SpaceSensor.scanf("%s", st); read_string(vecf,st); direction[0]=(int)vecf[0]; st[0]='\n'; } void SpaceSensor::ReadOversampleRate(int *Rate){ _SpaceSensor.puts(":144\n"); _SpaceSensor.scanf("%s", st); read_string(vecf,st); Rate[0]=(int)vecf[0]; st[0]='\n'; } void SpaceSensor::ReadRunningAveragePercent(float *Percent){ _SpaceSensor.puts(":145\n"); _SpaceSensor.scanf("%s", st); read_string(Percent,st); st[0]='\n'; } void SpaceSensor::ReadDesiredUpdateRate(float *rate){ _SpaceSensor.puts(":146\n"); _SpaceSensor.scanf("%s", st); read_string(rate,st); st[0]='\n'; } void SpaceSensor::ReadKalmanFilterCovarianceMatrix(float *CovarianceMatrix){ _SpaceSensor.puts(":147\n"); _SpaceSensor.scanf("%s", st); read_string(CovarianceMatrix,st); st[0]='\n'; } void SpaceSensor::ReadAccelerometerRange(int *Accelrange){ _SpaceSensor.puts(":148\n"); _SpaceSensor.scanf("%s", st); read_string(vecf,st); Accelrange[0]=(int)vecf[0]; st[0]='\n'; } void SpaceSensor::ReadMultiReferenceWeightPower(float *WeightPower){ _SpaceSensor.puts(":149\n"); _SpaceSensor.scanf("%s", st); read_string(WeightPower,st); st[0]='\n'; } void SpaceSensor::ReadMultiReferenceResolution(int *Resolution){ _SpaceSensor.puts(":150\n"); _SpaceSensor.scanf("%s", st); read_string(vecf,st); Resolution[0]=(int)vecf[0]; st[0]='\n'; } void SpaceSensor::ReadNumberOfmultiReferenceCells(int *NumberOfCells){ _SpaceSensor.puts(":151\n"); _SpaceSensor.scanf("%s", st); read_string(vecf,st); NumberOfCells[0]=(int)vecf[0]; st[0]='\n'; } void SpaceSensor::ReadFilterEnableState(int *Mode){ _SpaceSensor.puts(":152\n"); _SpaceSensor.scanf("%s", st); read_string(vecf,st); Mode[0]=(int)vecf[0]; st[0]='\n'; } void SpaceSensor::ReadRunningAverageMode(int *Mode){ _SpaceSensor.puts(":153\n"); _SpaceSensor.scanf("%s", st); read_string(vecf,st); Mode[0]=(int)vecf[0]; st[0]='\n'; } void SpaceSensor::ReadGyroscopeRange(int *mode){ _SpaceSensor.puts(":154\n"); _SpaceSensor.scanf("%s", st); read_string(vecf,st); mode[0]=(int)vecf[0]; st[0]='\n'; } void SpaceSensor::ReadCompassRange(int *mode){ _SpaceSensor.puts(":155\n"); _SpaceSensor.scanf("%s", st); read_string(vecf,st); mode[0]=(int)vecf[0]; st[0]='\n'; } /**********************************************************************************************************************/ /*4.3.7 Commands for Calibration*/ /**********************************************************************************************************************/ void SpaceSensor::SetCompassCalibrationParameters(float Bias[3],float Matrix[9]){ sprintf( st ,":160,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",Bias[0],Bias[1],Bias[2],Matrix[0],Matrix[1],Matrix[2],Matrix[3],Matrix[4],Matrix[5],Matrix[6],Matrix[7],Matrix[8]); _SpaceSensor.puts(st); st[0]='\n'; } void SpaceSensor::SetAccelerometerCalibrationParameters(float Bias[3],float Matrix[9]){ sprintf( st ,":161,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",Bias[0],Bias[1],Bias[2],Matrix[0],Matrix[1],Matrix[2],Matrix[3],Matrix[4],Matrix[5],Matrix[6],Matrix[7],Matrix[8]); _SpaceSensor.puts(st); st[0]='\n'; } void SpaceSensor::ReadCompassCalibrationParameters(float *Bias,float *Matrix){ _SpaceSensor.puts(":162\n"); _SpaceSensor.scanf("%s", st); read_string(vecf,st); for(int i=0;i<3;i++){Bias[i]=vecf[i];} for(int i=0;i<9;i++){Matrix[i]=vecf[i+3];} st[0]='\n'; } void SpaceSensor::ReadAccelerometerCalibrationParameters(float *Bias,float *Matrix){ _SpaceSensor.puts(":163\n"); _SpaceSensor.scanf("%s", st); read_string(vecf,st); for(int i=0;i<3;i++){Bias[i]=vecf[i];} for(int i=0;i<9;i++){Matrix[i]=vecf[i+3];} st[0]='\n'; } void SpaceSensor::ReadGyroCalibrationParameters(float *Bias, float *range){ _SpaceSensor.puts(":164\n"); _SpaceSensor.scanf("%s", st); read_string(vecf,st); for(int i=0;i<3;i++){Bias[i]=vecf[i];} for(int i=0;i<3;i++){range[i]=vecf[i+3];} st[0]='\n'; } void SpaceSensor::BeginGyroAutocalibration(){ _SpaceSensor.puts(":165\n"); } void SpaceSensor::SetGyroCalibrationParameters(float Bias[3], float range[3]){ sprintf( st ,":166,%f,%f,%f,%f,%f,%f\n",Bias[0],Bias[1],Bias[2],range[0],range[1],range[2]); _SpaceSensor.puts(st); st[0]='\n'; } /**********************************************************************************************************************/ /*4.3.8 General Commands*/ /**********************************************************************************************************************/ void SpaceSensor::ReadSoftwareVersion(char *version){ _SpaceSensor.puts(":223\n"); _SpaceSensor.scanf("%s", version); } void SpaceSensor::RestoreFactorySettings(){ _SpaceSensor.puts(":224\n"); } void SpaceSensor::CommitSettings(){ _SpaceSensor.puts(":225\n"); } void SpaceSensor::SoftwareReset(){ _SpaceSensor.puts(":226\n"); } void SpaceSensor::EnableWatchdogTimer(int timer){ sprintf( st ,":227,%d\n",timer); _SpaceSensor.puts(st); } void SpaceSensor::DisableWatchdogTimer(){ _SpaceSensor.puts(":228\n"); } void SpaceSensor::EnterFirmwareUpdateMode(){ _SpaceSensor.puts(":229\n"); } void SpaceSensor::ReadHardwareVersion(char *version){ _SpaceSensor.puts(":230\n"); _SpaceSensor.scanf("%s", version); } void SpaceSensor::SetUARTBaudRate(int Baudrate){ sprintf( st ,":231,%d\n",Baudrate); _SpaceSensor.puts(st); } void SpaceSensor::GetUARTBaudRate(int *Baudrate){ _SpaceSensor.puts(":232\n"); _SpaceSensor.scanf("%s", st); read_string(vecf,st); Baudrate[0]=(int)vecf[0]; } void SpaceSensor::SetUSBmode(int mode){ sprintf( st ,":233,%d\n",mode); _SpaceSensor.puts(st); } void SpaceSensor::GetUSBmode(int *mode){ _SpaceSensor.puts(":234\n"); _SpaceSensor.scanf("%s", st); read_string(vecf,st); mode[0]=(int)vecf[0]; } void SpaceSensor::SetClockSpeed(int clock){ sprintf( st ,":235,%d\n",clock); _SpaceSensor.puts(st); } void SpaceSensor::GetClockSpeed(int *clock){ _SpaceSensor.puts(":236\n"); _SpaceSensor.scanf("%s", st); read_string(vecf,st); clock[0]=(int)vecf[0]; } void SpaceSensor::GetSerialnumber(int *serialnum){ _SpaceSensor.puts(":237\n"); _SpaceSensor.scanf("%s", st); read_string(vecf,st); serialnum[0]=(int)vecf[0]; } void SpaceSensor::SetLEDColor(float color[3]){ sprintf( st ,":238,%f,%f,%f\n",color[0],color[1],color[2]); _SpaceSensor.puts(st); } void SpaceSensor::GetLEDColor(float *color){ _SpaceSensor.puts(":239\n"); _SpaceSensor.scanf("%s", st); read_string(color,st); } void SpaceSensor::EnableDisableJoystick(int Mode){ sprintf( st ,":240,%d\n",Mode); _SpaceSensor.puts(st); } void SpaceSensor::EnableDisablemouse(int Mode){ sprintf( st ,":241,%d\n",Mode); _SpaceSensor.puts(st); } void SpaceSensor::ReadJoystickEnabledState(int *Mode){ _SpaceSensor.puts(":242\n"); _SpaceSensor.scanf("%s", st); read_string(vecf,st); Mode[0]=(int)vecf[0]; } void SpaceSensor::ReadMouseEnabledState(int *Mode){ _SpaceSensor.puts(":243\n"); _SpaceSensor.scanf("%s", st); read_string(vecf,st); Mode[0]=(int)vecf[0]; } void SpaceSensor::SetControlMode(float Controlclass,float controlindex,float handlerindex){ sprintf( st ,":244,%f,%f,%f\n",Controlclass,controlindex,handlerindex); _SpaceSensor.puts(st); } void SpaceSensor::SetControlData(float Controlclass,float controlindex,float datapointindex,float datapoint ){ sprintf( st ,":245,%f,%f,%f,%f\n",Controlclass,controlindex,datapointindex,datapoint); _SpaceSensor.puts(st); } void SpaceSensor::ReadControlMode(float *Handlerindex, float Controlclass,float controlindex){ sprintf( st ,":246,%f,%f\n",Controlclass,controlindex); _SpaceSensor.puts(st); _SpaceSensor.scanf("%s", st); read_string(Handlerindex,st); } void SpaceSensor::ReadControlData(float *Datapoint,float ControlClass,float controlIndex,float dataPointIndex){ sprintf( st ,":247,%f,%f,%f\n",ControlClass,controlIndex,dataPointIndex); _SpaceSensor.puts(st); _SpaceSensor.scanf("%s", st); read_string(Datapoint,st); } void SpaceSensor::SetMouseAbsoluteRelative(int Mode){ sprintf( st ,":251,%d\n",Mode); _SpaceSensor.puts(st); } void SpaceSensor::ReadMouseAbsoluteRelative(int *Mode){ _SpaceSensor.puts(":252\n"); _SpaceSensor.scanf("%s", st); read_string(vecf,st); Mode[0]=(int)vecf[0]; } void SpaceSensor::SetjoystickAndMousePresentRemoved(int Mode){ sprintf( st ,":253,%d\n",Mode); _SpaceSensor.puts(st); } void SpaceSensor::ReadjoystickAndMousePresentRemoved(int *Mode){ _SpaceSensor.puts(":254\n"); _SpaceSensor.scanf("%s", st); read_string(vecf,st); Mode[0]=(int)vecf[0]; } /**********************************************************************************************************************/ /*Private functions*/ /**********************************************************************************************************************/ void SpaceSensor::read_string(float *vector,char *c){ char var[10]; int var_size=0,j=0,i=0; while(1){ if(j==strlen(c)){ vector[i]=atof(var); break; } if(c[j]==','){ vector[i]=atof(var); i++; j++; var_size=0; } var[var_size]=c[j]; j++; var_size++; } } void SpaceSensor::begin(long baud) { _SpaceSensor.baud(baud); }