int moveFront()
{
motorForward(1);
motorForward(2);
//delay(ms);
while(1)
{
if(botAtNode())
{
motorStop(1);
motorStop(2);
return OK;
}
if(botBlocked())
{
motorStop(1);
motorStop(2);
motorReverse(1);
motorReverse(2);
//delay(ms);
while(1)
{
if(botAtNode())
{
motorStop(1);
motorStop(2);
return BLOCK;
}
}
}
}
}
void Move::rotate(int angle) {
changeMoveState(MOV_ROTATE);
#ifdef MOVE_DEBUG
Serial.print(P("Rotating "));
Serial.println(angle);
#endif
if (angle < 0) {
#ifdef MOVE_DEBUG
Serial.println(P(" (left)"));
#endif
motorReverse(MOTOR_LEFT, speed);
motorForward(MOTOR_RIGHT, speed);
} else if (angle > 0) {
#ifdef MOVE_DEBUG
Serial.println(P(" (right)"));
#endif
motorForward(MOTOR_LEFT, speed);
motorReverse(MOTOR_RIGHT, speed);
}
int ms = rotationAngleToTime(angle);
movingDelay(ms);
brake();
}
void setMotorDirection(uint8_t direction)
{
if (direction == FORWARD) // if direction is foward
{
motorForward();
}
else if (direction == REVERSE) // if direction is reverse
{
motorReverse();
}
else // else set motor foward
motorForward();
}
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;
}
}
}
}
// 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);
}
int checkIfBalanced(){
int xValue, yValue, zValue;
float angle;
//Read the X axis
adcInit(ACCELEROMETER_X);
xValue = adcRead();
//Read the Y axis
adcInit(ACCELEROMETER_Y);
yValue = adcRead();
//Read the Z axis
adcInit(ACCELEROMETER_Z);
zValue = adcRead();
angle = computePitch(xValue, yValue, zValue);
//check what the angle is and if we need to go forward or backward
if(angle > FORWARD_THRESHOLD){
motorForward();
return FALSE;
}else if(angle < REVERSE_THRESHOLD){
motorReverse();
return FALSE;
}else{
motorStop();
return TRUE;
}
}
void Robot::moveRightWheelBackward(int power) {
if (RIGHTWHEELORIENTATION == 0) {
motorBackward(RIGHTWHEELMOTOR, power);
} else if (RIGHTWHEELORIENTATION == 1) {
motorForward(RIGHTWHEELMOTOR, power);
}
}
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);
}
}
// 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::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;
}
void main() {
initPIC16F88();
STATUS_LED = 1;
__delay_ms(500); //Required delay before startup
motorForward();
while(1){
if(checkIfBalanced() == TRUE)
{
//Stop processing
while(1){
__delay_ms(250);
STATUS_LED = 1;
__delay_ms(250);
STATUS_LED = 0;
}
}
/*Check if we are still on the ramp*/
if(!onRamp(LEFT_IR_SENSOR)){
motorRight();
}else if(!onRamp(RIGHT_IR_SENSOR))
{
motorLeft();
}
else{
motorForward();
}
}
return;
}
void motorDown(char* distance){
mm = atoi(distance);
readingsTaken = 0;
readingsWanted = mm * 9.449; //Each mm of wire correlates to about 9.449 readings
//Loop to control motor for desired change in height
while(readingsTaken < readingsWanted){
motorForward();
//Checks for low signal, then high signal, then increments readingTaken
while(!(PINA & 0x08)){}
while(PINA & 0x08){}
readingsTaken++;
}
}
void turnRight()
{
motorReverse(2);
motorForward(1);
//delay(ms);
while(1)
{
if(botAtNode())
{
motorStop(1);
motorStop(2);
return;
}
}
}
void motorSquare(uint16_t peakVoltage)
{
uint16_t zero = 0; // 16 bit zero
motorForward();
setMotorAmplitude(peakVoltage); // drive the motor at DC voltage
__delay_cycles(2000000); // 1 second delay
setMotorAmplitude(zero); // stop motor
motorOpen(); // bring motor to open state as recomended from datasheet
__delay_cycles(20); // let voltage levels settle
motorBrake();
__delay_cycles(2000000);
// break motor
//while(ADC12MEM0 > 100){}
//ADC12IER0 &= ~ADC12IE0; // disable interupt on ADC12IFG0 bit
//ADC12IER0 &= ~ADC12IE1; // disable interupt on ADC12IFG1 bit
//startTimer();
}
void motorDown(char* distance) {
mm = atoi(distance);
readingsTaken = 0;
readingsWanted = mm * encoderCount; //Each mm of wire correlates to about 9.449 readings
if(currentPosition <511){
motorForward();
_delay_ms(20);
estimateCounter = 0;
estimatedPosition = currentPosition;
while(readingsTaken < readingsWanted) {
if(estimatedPosition > 584){
//sysTransmitString(terminalUartIndex, "At minimum height.\n");
motorOff();
break;
}
//Checks for low signal, then high signal, then increments readingTaken
while(PINA & _BV(PA6)){}
while(!(PINA & _BV(PA6))){}
readingsTaken++;
estimateCounter++;
//Increments estimated counter every 9 readings taken
if(estimateCounter == 4){
estimatedPosition++;
estimateCounter = 0;
}
}
//sysTransmitString(terminalUartIndex, "Motor Done.\n");
} else{
//sysTransmitString(terminalUartIndex, "At minimum height.\n");
}
//Changes the systems idea of where the light hood is
//Only changes if the system has been zeroed already
if(systemReset == TRUE){
currentPosition = currentPosition + (readingsTaken/encoderCount);
}
motorOff();
}
void Move::spinRight() {
motorForward(MOTOR_LEFT, speed);
motorReverse(MOTOR_RIGHT, speed);
}
void Move::forward() {
changeMoveState(MOV_FORWARD);
motorForward(MOTOR_LEFT, speed);
motorForward(MOTOR_RIGHT, speed);
}
void Move::right() {
changeMoveState(MOV_RIGHT);
motorForward(MOTOR_LEFT, speed);
motorForward(MOTOR_RIGHT, 0);
}
