//TODO shouldnt these share one function and just input the differences?
void Shieldbot::leftMotor(char mag){
int actualSpeed = 0;
if(mag >0){ //forward
float ratio = (float)(mag)/128;
actualSpeed = (int)(ratio*speedmotorB); //define your speed based on global speed
#if SHIELDBOTDEBUG
Serial.print("forward left: ");
Serial.println(actualSpeed);
#endif
analogWrite(speedPinLeft,actualSpeed);
digitalWrite(left1,HIGH);
digitalWrite(left2,LOW);//turn left motor counter-clockwise
}else if(mag == 0){ //neutral
#if SHIELDBOTDEBUG
Serial.println("nuetral left");
#endif
stopLeft();
}else { //meaning backwards
float ratio = (float)abs(mag)/128;
actualSpeed = ratio*speedmotorB;
#if SHIELDBOTDEBUG
Serial.print("backward left: ");
Serial.println(actualSpeed);
#endif
analogWrite(speedPinLeft,actualSpeed);
digitalWrite(left1,LOW);
digitalWrite(left2,HIGH);//turn left 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 care up for any kind of left turn.
*/
void turnLeft(){
rightOn();
stopLeft();
}
