//char is 128 to 127
//mag is the direction to spin the right motor
//-128 backwards all the way
//0 dont move
//127 forwards all the way
void Shieldbot::rightMotor(char mag){
int actualSpeed = 0;
if(mag >0){ //forward
float ratio = (float)mag/128;
actualSpeed = (int)(ratio*speedmotorA); //define your speed based on global speed
#if SHIELDBOTDEBUG
Serial.print("forward right: ");
Serial.println(actualSpeed);
#endif
analogWrite(speedPinRight,actualSpeed);
digitalWrite(right1,HIGH);
digitalWrite(right2,LOW);//turn right motor clockwise
}else if(mag == 0){ //neutral
#if SHIELDBOTDEBUG
Serial.println("nuetral right");
#endif
stopRight();
}else { //meaning backwards
float ratio = (float)abs(mag)/128;
actualSpeed = ratio*speedmotorA;
#if SHIELDBOTDEBUG
Serial.print("backward right: ");
Serial.println(actualSpeed);
#endif
analogWrite(speedPinRight,actualSpeed);
digitalWrite(right1,LOW);
digitalWrite(right2,HIGH);//turn right motor counterclockwise
}
}
/*
* Pauses the car for one second
*/
void pauseBoth(){
stopRight();
stopLeft();
__delay_cycles(STRAIGHTTIME);
rightOn();
leftOn();
}
/*
* Stops the car until further notice.
*/
void stopBoth(){
stopRight();
stopLeft();
__delay_cycles(STRAIGHTTIME);
//rightOn();
//leftOn();
}
void stopAll(){
if(move){
float rot = getCompassValue();
float x = sinDegrees(rot) * encoder;
float y = cosDegrees(rot) * encoder;
nxtDisplayTextLine(0, "%d %d %d", rot, x, y);
nxtDisplayTextLine(1, "%d", encoder);
totalX+=x;
totalY+=y;
nMotorEncoder[motorB] = 0;
move = false;
}
stopBack();
stopF();
stopLeft();
stopRight();
motor[clawm] = 0;
}
void Shieldbot::stop(){
stopLeft();
stopRight();
}
/*
* Sets the car up for any kind of right turn
*/
void turnRight(){
leftOn();
stopRight();
}
