void about_face(){
rightForward(20);
leftBackward(30);
delay(1000);
rightForward(0);
leftForward(0);
}
void drive_test(){
int leftDistance = readLeft();
if(leftDistance > 80) {
leftBackward(30);
rightBackward(30);
}
else if( leftDistance < 70 ) {
rightForward(30);
leftForward(30);
}
}
// the setup routine runs once when you press reset:
void setup()
{
pinMode(RenchA, INPUT);
pinMode(RenchB, INPUT);
pinMode(LenchA, INPUT);
pinMode(LenchB, INPUT);
//initialize motor pins to off
pinMode(R_fwd, OUTPUT); //initialize rightmotor forward as output
pinMode(R_bkw, OUTPUT);
pinMode(L_fwd, OUTPUT);
pinMode(L_bkw, OUTPUT);
Serial.begin(9600); //initialize for serial output
//power for the receivers *leave as is DO NOT CHANGE*
pinMode(A14, OUTPUT);
analogWrite(A14, 255);
//initialize debug LEDs
pinMode(led1, OUTPUT); //setup led1 for output
pinMode(led2, OUTPUT); //setup led2 for output
pinMode(led3, OUTPUT); //setup led3 for output
//setup sensor pins for output and input
pinMode(LF_Emitter, OUTPUT); //initializes leftfront emitter to output stuff
pinMode(RF_Emitter, OUTPUT);
pinMode(LF_Receiver, INPUT); //initialize leftfront receiver to receive input from stuff
pinMode(RF_Receiver, INPUT);
pinMode(SIDE_HIGH_POWER, OUTPUT);
pinMode(L_Receiver, INPUT);
pinMode(R_Receiver, INPUT);
//setup the speaker pin
pinMode(speakerPin, OUTPUT);
rightForward(0);
leftForward(0);
}
/**** PID TEST ****/
void drive_straight_PID(void){
int offset = -40;
static int previous_error = 0;
static int previous_time = 0;
static int last_big = 0;
int error; //current error values
int biggest;
int current_time; //current time
double total;
int leftDiagSensor, rightDiagSensor;
double kp = 0.5, kd = 0.5;
leftDiagSensor = readLeft();
rightDiagSensor = readRight();
//debug print out sensor readings
//Serial.print("IR left diag: ");
//Serial.print(leftDiagSensor);
//Serial.print(" IR right diag: ");
//Serial.print(rightDiagSensor);
if(!previous_time)
{
previous_time = millis();
return;
}
leftDiagSensor = readLeft();
rightDiagSensor = readRight();
if( 1 )//temporarily for walls on both sides only |x|
{
error = rightDiagSensor - leftDiagSensor + offset;
}
total = error *kp;
previous_time = current_time;
//analogWrite(R_fwd, HIGH - total);
//analogWrite(L_fwd, HIGH + total);
//what the PID will do (because motor functions are not done)
if(total > 25)
total=0.5*total;
if(total > 50 )
total = 0;
if(total<-50)
total=0;
Serial.print("total error: ");
Serial.println(total);
rightForward(15+total);
leftForward(25-total);
if( error == 0 ){
Serial.print(" Mouse is straight: ");
Serial.println(error);
}
if( error > 0 ){
Serial.print(" Mouse is veering right: ");
Serial.println(error);
}
if( error < 0 ){
Serial.print(" Mouse is veering left: ");
Serial.println(error);
}
}//end drive_straight_PID
void turnRight(){
rightBackward();
leftForward();
}
void goForward(){
rightForward();
leftForward();
}
void pivotForwardRight(){
leftForward();
__delay_cycles(6000000);
stop();
}
