roboforum.ru

#include <ORMotor.h>
#include <ORBWSens.h>

ORBWSens bws((unsigned char[])
  {3, 4, 5, 6, 7, 14, 15, 16, 17, 18}, 10,
  4000,
  2 /* LED-on pin */);

ORMotor LeftMotor(ORDUINO_MOTOR_B);
ORMotor RightMotor(ORDUINO_MOTOR_A);

const int calSpd = 40;
const int calSteps = 40;

void setup()
{
  Serial.begin(115200);
  LeftMotor.speed(-calSpd);
  RightMotor.speed(calSpd);
  for (int i=0; i < calSteps; i++)
  {
    bws.calibrate();
  }
  LeftMotor.speed(calSpd);
  RightMotor.speed(-calSpd);
  for (int i=0; i < calSteps; i++)
  {
    bws.calibrate();
  }
  LeftMotor.speed(-calSpd);
  RightMotor.speed(calSpd);
  delay(700);
  LeftMotor.speed(0);
  RightMotor.speed(0);
}

unsigned int val[10];

const int fs = 50,
          ms = 30,
          ls = 10;

void loop()
{
  int mlSpd = ls, mrSpd = ls;
  int line = bws.readLine(val);
  int error = line - 4500;
 
  if (error > -1000 && error < +1000) {
    // line at center
    mlSpd = fs;
    mrSpd = fs;
  } else if (error > +1000 && error < +2000) {
    // right
    mlSpd = fs;
    mrSpd = ms;
  } else if (error > +2000 && error < +3000) {
    // right
    mlSpd = fs;
    mrSpd = ls;
  } else if (error > +3000) {
    // right
    mlSpd = ms;
    mrSpd = 0;
  } else if (error < -1000 && error > -2000) {
    // left
    mlSpd = ms;
    mrSpd = fs;
  } else if (error < -2000 && error > -3000) {
    // left
    mlSpd = ls;
    mrSpd = fs;
  } else if (error < -3000) {
    // left
    mlSpd = 0;
    mrSpd = ms;
  }
  
  LeftMotor.speed(mlSpd);
  RightMotor.speed(mrSpd);
  
#if 0
  Serial.print("LINE: ");
  Serial.print(line);
  Serial.print(" E: ");
  Serial.print(error);
  Serial.print(" L: ");
  Serial.print(mlSpd);
  Serial.print(" R: ");
  Serial.println(mrSpd);
#endif

  delay(10);
}