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();
}
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;
}
}
}
}
}
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 motorReset(void) {
//motorReverse(); Pending tests
//loop_until_bit_is_clear(PINA,PA3)
while(PINB & _BV(PB1)) {
motorReverse();
}
motorOff();
systemReset = TRUE;
currentPosition = 0;
//sysTransmitString(terminalUartIndex, "Height Reset\n");
}
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 turnRight()
{
motorReverse(2);
motorForward(1);
//delay(ms);
while(1)
{
if(botAtNode())
{
motorStop(1);
motorStop(2);
return;
}
}
}
void motorUp(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){
motorReverse();
//Checks encoder for low signal, then high signal, then increments readingTaken
while(!(PINA & 0x08)){}
while(PINA & 0x08){}
readingsTaken++;
}
}
void Move::spinRight() {
motorForward(MOTOR_LEFT, speed);
motorReverse(MOTOR_RIGHT, speed);
}
void Move::backward() {
changeMoveState(MOV_BACK);
motorReverse(MOTOR_LEFT, speed);
motorReverse(MOTOR_RIGHT, speed);
}
void motorUp(char* distance) {
mm = atoi(distance);
readingsTaken = 0;
readingsWanted = mm * encoderCount;
//Ensures bump sensor isn't active
if(PINB & _BV(PB1)){
motorReverse();
_delay_ms(20);
while(readingsTaken < readingsWanted) {
//Checks each loop to see if bump sensor hit
if(PINB & _BV(PB1)){
//Checks encoder for high signal
while(PINA & _BV(PA6)){
if(!(PINB & _BV(PB1))){
//Bump sensor hit, turn off motor and end loop
//sysTransmitString(terminalUartIndex, "Roof hit.\n");
motorOff();
systemReset = TRUE;
currentPosition = 0;
return;
}
}
//Checks encoder for low signal
while(!(PINA & _BV(PA6))){
if(!(PINB & _BV(PB1))){
//Bump sensor hit, turn off motor and end loop
//sysTransmitString(terminalUartIndex, "Roof hit.\n");
motorOff();
systemReset = TRUE;
currentPosition = 0;
return;
}
}
readingsTaken++;
} else{
//Bump sensor hit, turn off motor and end loop
//sysTransmitString(terminalUartIndex, "Roof hit - ");
motorOff();
systemReset = TRUE;
currentPosition = 0;
return;
}
}
//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);
}
//sysTransmitString(terminalUartIndex, "Motor Done.\n");
}
else{
//sysTransmitString(terminalUartIndex, "At roof.\n");
systemReset = TRUE;
currentPosition = 0;
}
motorOff();
}