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(); }