// Turn Secondary arm in to an specific angle between -110 to 110 degree: // Status: Untested void secondaryArmPosition(int angle){ if(angle > NUM_POTENTIOMETER_1_MAX_ANGLE || angle < NUM_POTENTIOMETER_1_MIN_ANGLE){ //Check if the input angle is posible return; } int diff =angle - checkSecondaryArmAngle(); //calculate the difference if(diff<5&&diff>-5){ Serial.println("Secondary Arm is not going to turn."); Serial.print("Diff:");Serial.println(diff); } else if(diff>0){ m1.forward(255); } else if(diff<0){ m1.backward(255); } while(diff>5||diff<-5){ diff =angle - checkSecondaryArmAngle(); } m1.brake(); }
// Turn the Grabber in to an specific angle between -110 to 110 degree: // Status: Untested void grabBasePosition(int angle) { if(angle > NUM_POTENTIOMETER_2_MAX_ANGLE || angle < NUM_POTENTIOMETER_2_MIN_ANGLE){ //Check if the input angle is posible return; } int diff =angle - checkGrabAngle(); //calculate the difference if(diff<5&&diff>-5){ Serial.println("Grab is not going to turn."); Serial.print("Diff:");Serial.println(diff); } else if(diff<0){ m2.backward(200); //Here is Backward!!!! } else if(diff>0){ m2.forward(200); } while(diff>5||diff<-5){ diff =angle - checkGrabAngle(); } m2.brake(); }
/* * Make the robot come to a full stop */ void MotorController::doBrake() { _processingNewCommand = true; leftMotor.brake(); rightMotor.brake(); }
void MotorControl::brake(float power) { motor1->brake(power); motor2->brake(power); motor3->brake(power); }