// moveTicks moves the number of ticks given.
// A positive ticks number will go forward, a negative ticks number
// will go backwards.
// We do not stop after hitting the number of ticks. Call stopMoving() to do so.
void Robot::moveTicks(int ticks, int motorSpeed)
{
wheelEnc.getCountsAndResetM1();
wheelEnc.getCountsAndResetM2();
bool moveComplete = false;
while(!moveComplete)
{
int motorOne = abs(wheelEnc.getCountsM1());
int motorTwo = abs(wheelEnc.getCountsM2());
int error = errorAdjustment();
if(motorTwo > abs(ticks) || motorOne > abs(ticks))
{
moveComplete = true;
}
else
{
if(ticks > 0)
{
// motorForward(adjustedMotorSpeed, error);
motorForward(motorSpeed, error);
}
else
{
// motorBackward(adjustedMotorSpeed, error);
motorBackward(motorSpeed, error);
}
}
delay(1);
}
}
void Robot::moveRightWheelBackward(int power) {
if (RIGHTWHEELORIENTATION == 0) {
motorBackward(RIGHTWHEELMOTOR, power);
} else if (RIGHTWHEELORIENTATION == 1) {
motorForward(RIGHTWHEELMOTOR, power);
}
}
// TODO at the moment, this only works well for NORTH, SOUTH, EAST, WEST
// We'll try to sort it out for NEAST, SEAST, NWEST, SWEST later.
void Robot::moveTillPoint(int motorSpeed, bool forward, bool useBothSensors)
{
wheelEnc.getCountsAndResetM1();
wheelEnc.getCountsAndResetM2();
//Moves forward or backward based on the boolean 'forward' until a line intersection.
//unsigned int sensors[8];
//gridSensors.readLine(sensors, QTR_EMITTERS_ON, true);
int error;
//bool atPoint = false;
while(!reachedPoint(useBothSensors)/*!atPoint*/)
{
/* gridSensors.readLine(sensors, QTR_EMITTERS_ON, true);
if(reachedPoint(sensors, useBothSensors))
{
delay(5);
gridSensors.readLine(sensors, QTR_EMITTERS_ON, true);
if(reachedPoint(sensors, useBothSensors))
atPoint = true;
}*/
error = errorAdjustment();
if (forward)
motorForward(motorSpeed, error);
else
motorBackward(motorSpeed, error);
}
if(!useBothSensors)
moveTicks(5,STRAIGHTSPEED);
}
void Robot::moveLeftWheelBackward(int power) {
if (LEFTWHEELORIENTATION == 0) {
motorBackward(LEFTWHEELMOTOR, power);
} else if (LEFTWHEELORIENTATION == 1) {
motorForward(LEFTWHEELMOTOR, power);
}
}
void CommandSet::grab()
{
const int direction = atoi(sCmd.next());
if (state.grabber_state != Open && state.grabber_state != Closed) {
Serial.println(F("N - grab"));
return;
}
Serial.println(F("A"));
#ifdef FW_DEBUG
Serial.print(F("grabbing "));
Serial.println(direction);
#endif
const pid_t pid = state.grab_handler->id;
const int motor_power = 200;
/* If we're open, doing that again will bugger the vision plate */
if (direction) {
state.grabber_state = Closing;
motorForward(grabber_port, motor_power);
processes.change(pid, 300L);
} else if (state.grabber_state != Open) {
state.grabber_state = Opening;
motorBackward(grabber_port, motor_power);
processes.change(pid, 280L);
} else {
return;
}
processes.enable(pid);
processes.forward(pid);
}
void Robot::moveGrabberBackward(int power) {
if (GRABBERORIENTATION == 0) {
motorBackward(GRABBERMOTOR, power);
} else if (GRABBERORIENTATION == 1) {
motorForward(GRABBERMOTOR, power);
}
}
void Robot::moveKickerBackward(int power) {
if (KICKERORIENTATION == 0) {
motorBackward(KICKERMOTOR, power);
} else if (KICKERORIENTATION == 1) {
motorForward(KICKERMOTOR, power);
}
}
void testMotor(void)
{
char c;
Serialflush();
Serialprint("Motor Test\n");
while(1)
{
if(uartNewLineFlag == 1)
{
Serialprint("LF received\n");
c = uartReadBuffer[0];
Serialflush();
switch(c)
{
case 'a': motorForward();
Serialprint("Motor FW\n");
break;
case 'b': motorBackward();
Serialprint("Motor BW\n");
break;
case 'c': motorLeft();
Serialprint("Motor Left\n");
break;
case 'd': motorRight();
Serialprint("Motor Right\n");
break;
case 'e': motorStop();
Serialprint("Motor Stop\n");
break;
}
}
}
}
void Robot::ungrab() {
if (GRABBERORIENTATION == 0) {
motorBackward(GRABBERMOTOR, 80);
} else if (GRABBERORIENTATION == 1) {
motorForward(GRABBERMOTOR, 80);
}
ungrabbing = true;
grabberTimer = 0;
}
void Robot::kickBackward() {
if (KICKERORIENTATION == 0) {
motorBackward(KICKERMOTOR, 100);
} else if (KICKERORIENTATION == 1) {
motorForward(KICKERMOTOR, 100);
}
unkicking = true;
kickerTimer = 0;
}
void Robot::kickForward(int power) {
ungrab();
if (KICKERORIENTATION == 0) {
motorForward(KICKERMOTOR, 100);
} else if (KICKERORIENTATION == 1) {
motorBackward(KICKERMOTOR, 100);
}
kicking = true;
kickerTimer = 0;
}
