// 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; }