Mindboards

Code: Select all

float FindOffset()
{
     float tot = 0;
    repeat (1000)
    {
        tot += SensorHTGyro(S1, 0); //tot var is 1000 gyro readings
    }
    PlayTone(440,500);
    return tot / 1000;   // returns avg
}

Code: Select all

//--------------------------------------------------
// IO functions, sensors
//--------------------------------------------------

#define Menc(a) MotorRotationCount(a)

#define Beep(f,d) PlayTone(f,d)

#define printf1( _x, _y, _format1, _value1) { \
  string sval1 = FormatNum(_format1, _value1); \
  TextOut(_x, _y, sval1); \
}

const char LCDline[]={56,48,40,32,24,16,8,0};

const string clrln="                 ";

inline bool btnhit(){
   return ( ButtonPressed(BTN1, false) || ButtonPressed(BTN2, false)
         || ButtonPressed(BTN3, false) || ButtonPressed(BTN4, false));
}


void PressToContinue(char Button) {
   string msg;
   if (Button==BTNCENTER)   msg="press BtnCntr...";
   else
   if (Button==BTNEXIT)     msg="press BtnExit...";
   else
   if (Button==BTNRIGHT)    msg="press BtnRight...";
   else
   if (Button==BTNLEFT)     msg="press BtnLeft...";

   printf1(0,LCDline[7],"%s", msg);
   while (!ButtonPressed(Button, false)); while (btnhit());
}

//--------------------------------------------------
// math
//--------------------------------------------------

#define min(a,b) (a<b?a:b)

#define max(a,b) (a>b?a:b)


inline long round(float f)
{
  if (f>=0) return (f + 0.5);
  else  return (f - 0.5);
}

inline float ArrayMedianF(float src[], int len)
{
  float ftemp[];

  ArraySort(ftemp, src, NA, NA)
  return ftemp[(len-1)/2];
}


inline void ArrayPushF(float &src[], float _new, int len)
{
  for (int i=len; i>0; --i) {src[i]=src[i-1];} // shift up
  src[0]=_new;
}


//--------------------------------------------------
// sensors
//--------------------------------------------------

#define GYRO    S2
#define COMPASS S3

long  GyroHeading,
      CompHeading, CompOffset,
      MencHeading ;
float GyroOffset=0.0,  GyroValue, GyroIntegral;


//--------------------------------------------------
// sensor calibration
//--------------------------------------------------

void FindGyroOffset()
{
    float tot = 0;
    for (int i=0; i<500; ++i)
    {
       tot += SensorHTGyro(GYRO, 0); // tot var is 500 gyro readings
       if (!(i%100)) Beep(1760,10);
       Wait(5);                     // Wait 5 ms (analog sensor)
    }
    PlayTone(440,100);
    GyroOffset= (tot/500);           //  avg
}


//------------------------------------------------------------------------------


task GetRotation(){

  long  GyroCalib=10500, temp;
  float gyrodata[5];
  
  CompHeading=CompOffset=SensorHTCompass(COMPASS);

  FindGyroOffset();           // approximation of Gyro Offset

  temp=0;
  GyroIntegral=0;

  while(1) {
     GyroValue=SensorHTGyro(GYRO,0)-GyroOffset;
     ArrayPushF(gyrodata, GyroValue, 5);
     GyroValue= (ArrayMean(gyrodata, 0, 5));

     GyroIntegral+=GyroValue;

     GyroHdg=round(GyroIntegral*(360.0/GyroCalib));

     if (ButtonPressed(BTNLEFT, false)) {   // press  BTNLEFT:
        GyroIntegral=0;                     // reset GyroIntegral
        while (btnhit()); Beep(1000,10);
                                            // now turn robot 360°
     }                                      // then press BTNRIGHT:
     if (ButtonPressed(BTNRIGHT, false)) {  // reset GyroCalib after 360° turn
        GyroCalib=abs(GyroIntegral);
        while (btnhit());  Beep(1000,10);
     }

     while (GyroHdg>=360) GyroHdg-=360;
     while (GyroHdg<  0)  GyroHdg+=360;
     
     CompHeading=SensorHTCompass(COMPASS);
     
     Wait(20);
  }
}

task DisplayValues() {
  while (1) {
     printf1(0, 56, "mot_B%6d", Menc(OUT_B));
     printf1(0, 48, "mot_C%6d", Menc(OUT_C));

     printf1(0, 40, "GValu%6.1f", GyroValue);
     printf1(0, 32, "GIntg%6.1f", GyroIntegral);
     printf1(0, 24, "GOffs%6.1f", GyroOffset);

     printf1(0, 16, "MotoH%6d", MencHeading);
     printf1(0,  8, "CompH%6d", CompHeading);
     printf1(0,  0, "GyroH%6d", GyroHeading);
     Wait(10);
  }
}

task main(){

  SetSensorHTGyro(GYRO);
  SetSensorLowspeed(COMPASS);

  start DisplayValues;
  start GetRotation;

  while(1);
}