MotorTest/test.c

gehe zur Dokumentation dieser Datei

/***************************************************************************
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   any later version.                                                    *
 ***************************************************************************/
// testroutine for new wheelspeed, turn, go function
// 29.7.2005
// 19.12.2005 m.a.r.v.i.n speed parameter for go function added
// 20.12.2005 m.a.r.v.i.n speed parameter for turn function and variable Wheelspeed added
#include "asuro.h"

unsigned char Wheelspeed[2];

#define aus MotorDir(BREAK,BREAK)
void testfahrt(void)
{
  unsigned char s1=100,s2=100;

  MotorSpeed(150,150);
  MotorDir(FWD,FWD);

  while (1)
  {
    if (Wheelspeed[0]<150)s1++;
    if (Wheelspeed[0]>160)s1--;
    if (Wheelspeed[1]<150)s2++;
    if (Wheelspeed[1]>160)s2--;

    Msleep(2);
    MotorSpeed(s1,s2);

    SerWrite("\n\r  speed Left,Right ",21);

    PrintInt(Wheelspeed[0]);
    PrintInt(Wheelspeed[1]);
  }
}
void treasure(void)
{
  Go(350,150);
  Go(-50,150);
  Msleep(1000);

  Turn(90,150);
  Go(100,150);
  Turn(-90,150);
  Go(170,150);
  Turn(-90,150);
  Go(100,150);
  Turn(-90,150);

  Go(470,150);
  Go(-50,150);

  Msleep(1000);
  Turn(90,150);
  Go(100,150);
  Turn(-90,150);
  Go(170,150);
  Turn(-90,150);
  Go(100,150);
  Turn(-90,150);
}

int main(void)
{
  int n;

  Init();
  EncoderInit();
  StatusLED(OFF);
  SerWrite("\n\r motor calibration V0.1 \n\r",28);
  while (1)
  {
    n=PollSwitch ();
    if (n==8)
    {
      SerWrite("\n\r testfahrt \n\r",15);
      Msleep(3000);
      testfahrt();
    }
    if (n==16)
    {
      SerWrite("\n\r treasure \n\r",14);
      Msleep(3000);
      treasure();
    }

    StatusLED(RED);
    Msleep(500);
    StatusLED(GREEN);
    Msleep(500);
  }
  return 0;
}

---