void stop_Wheel(char Wheel) {
// User gives which wheel to stop as a char
switch (Wheel)
{
case 'L': // User specifies left wheel
case 'l':
digitalWrite(pinCC_Left, LOW);
digitalWrite(pinCW_Left, LOW);
break;
case 'R': // User specifies right wheel
case 'r':
digitalWrite(pinCC_Right, LOW);
digitalWrite(pinCW_Right, LOW);
break;
case 'B': // User specifies both wheels
case 'b':
all_Stop();
break;
default : // default is to stop both
Serial.println("\n\n\n\n\nSomething happened\n\n\n\n\n");
all_Stop();
digitalWrite(13, HIGH); // turn on LED if something went wrong
break;
}
}
void preciseRobotStraight(float distance, int wheel_speed, boolean wheel_Direction)
{ // Slows down at 80%
Serial.println("The robot is now going in a 'precise' straight line.\n");
int step = 1;
long target = encoderDistance(distance);
switch (step)
{
case 1: // The wheel is going at normal speed until 80% distance
while ((count_Left <= (target * 0.8)) && (count_Right <= (target * 0.8)))
{
wheel_go(wheel_speed, 'B', wheel_Direction);
}
break;
case 2: // Wheel is going slower now
while ((count_Left <= target) && (count_Right <= target))
{
slow_Down('B');
}
break;
default:
Serial.println("\n\n\n\n\nSomething happened\n\n\n\n\n");
digitalWrite(13, HIGH); // turn on LED if something went wrong
all_Stop();
break;
}
}
void rightTurn()
{
stepNumber=1;
boolean finished=false;
while(!finished){
switch (stepNumber)
{
case 1:
Serial.println("Now at Step 1:\n");
delay(1000);
stepNumber = 2;
break;
case 2:
Serial.println("Now at Step 2:\n");
wheel_go(255, 'L', true); // Start wheel at max speed counterclockwise
wheel_go(255,'R',false);
if ((count_Left >= TURN_MAX)&&(count_Right>=TURN_MAX))stepNumber = 3; // Slow down when near target
break;
case 3:
Serial.println("Now at Step 3:\n");
all_Stop();
//DDRB=0x00;
delay(1000);
count_Left = 0; // reset counter
count_Right = 0;
finished=true;
break;
}
}
}
void slow_Down(char Wheel) {
switch (Wheel) // Choose which wheel to slow down
{
case 'L':
case 'l':
analogWrite(pinSpeed_Left, 26);
break;
case 'R':
case 'r':
analogWrite(pinSpeed_Right, 26);
break;
case 'B':
case 'b':
analogWrite(pinSpeed_Left, 26);
analogWrite(pinSpeed_Right, 26);
break;
default:
digitalWrite(13, HIGH); // turn on LED if something went wrong
Serial.println("\n\n\n\n\nSomething happened\n\n\n\n\n");
all_Stop();
break;
}
}
void robotStraight(float distance, int wheel_speed)
{
long target = encoderDistance(distance); // Target should be the number of encoder pulses needed to travel a 'distance' distance
while ((count_Left <= target) && (count_Right <= target))
{
wheel_go(wheel_speed,'B',true);
}
Serial.println("The count has been reached");
all_Stop();
reset();
return;
}
CMMSound::~CMMSound(){
all_Stop();
}
void wheel_go(int wheel_speed, char Wheel, boolean wheel_Direction) {
// sets the given wheel at the wheel_speed with analogWrite()
// true for clockwise
// false for counterclockwise
switch (Wheel)
{
case 'L':
case 'l':
if (wheel_Direction) {
// Make wheel go clockwise
digitalWrite(pinCC_Left, LOW);
digitalWrite(pinCW_Left, HIGH);
}
if (!wheel_Direction) {
// Make wheel go counter-clockwise
digitalWrite(pinCW_Left, LOW);
digitalWrite(pinCC_Left, HIGH);
}
analogWrite(pinSpeed_Left, 0.75*wheel_speed); // actually sets the wheel speed
break;
case 'R':
case 'r':
if (wheel_Direction) { // TRUE wheel_Direction is clockwise
// Make wheel go clockwise
digitalWrite(pinCC_Right, LOW); // Turn off counter-clockwise right
digitalWrite(pinCW_Right, HIGH); // Turn on clockwise right
}
if (!wheel_Direction) {
// Make wheel go counter-clockwise
digitalWrite(pinCW_Right, LOW); // Turn off clockwise right
digitalWrite(pinCC_Right, HIGH); // Turn on counter-clockwise right
}
analogWrite(pinSpeed_Right, wheel_speed);
break;
case 'B': // User enters 'B'
case 'b': // User can also enter 'b'
if (wheel_Direction) { // TRUE wheel_Direction is clockwise
// Make both wheels go clockwise
digitalWrite(pinCC_Right, LOW); // Turn off counter-clockwise right
digitalWrite(pinCC_Left, LOW); // Turn off counter-clockwise left
digitalWrite(pinCW_Right, HIGH); // Turn on clockwise right
digitalWrite(pinCW_Left, HIGH); // Turn on clockwise left
}
if (!wheel_Direction) { // FALSE wheel_Direction is counter-clockwise
// Make both wheels go clockwise
digitalWrite(pinCW_Right, LOW); // Turn off clockwise right
digitalWrite(pinCW_Left, LOW); // Turn off clockwise left
digitalWrite(pinCC_Right, HIGH); // Turn on counter-clockwise right
digitalWrite(pinCC_Left, HIGH); // Turn on counter-clockwise left
}
analogWrite(pinSpeed_Left, wheel_speed)); // actually sets the wheel speed
analogWrite(pinSpeed_Right, wheel_speed); // actually sets the wheel speed
break;
default: // Should only reach here if coder error
Serial.println("\n\n\n\n\nSomething happened\n\n\n\n\n");
digitalWrite(13, HIGH); // turn on LED if something went wrong
all_Stop();
break;
}
}
