/*
* testRangeFinders()
* This function is used to test and classify the idealized function for the range finders, as well as
* implementing the basic functionality. It will turn LEDs on and off depending on if an object comes
* within a certain range (see spreadsheet for distance and readouts). Setting a breakpoint at the top
* of the while loop while the code is running will give a good value for what the ADC reads at a particular
* distance.
*/
void testRangeFinders()
{
BCSCTL1 = CALBC1_8MHZ;
DCOCTL = CALDCO_8MHZ;
initRangeFinders();
P1DIR |= LEFT_LED|RIGHT_LED;
int fBuffer[BUFFER_LN];
int lBuffer[BUFFER_LN];
int rBuffer[BUFFER_LN];
int mf;
int ml;
int mr;
fillBuffers(fBuffer, lBuffer, rBuffer);
while(1)
{
readFront(fBuffer); // Place Breakpoint here
readLeft(lBuffer);
readRight(rBuffer);
mf = median(fBuffer); // Medians were chosen as they are less influenced by outliers
ml = median(lBuffer); // The user is free to change these functions to test and classify the
mr = median(rBuffer); // means, however they should be close to the median values.
if(mf > 0x01F0)
{
P1OUT |= (RIGHT_LED|LEFT_LED);
}
else if(ml > 0x0220)
{
P1OUT &= ~RIGHT_LED;
P1OUT |= LEFT_LED;
}
else if(mr > 0x0220)
{
P1OUT &= ~LEFT_LED;
P1OUT |= RIGHT_LED;
}
else
{ // Important reset condition, do not forget in robot mevement code
P1OUT &= ~(LEFT_LED|RIGHT_LED);
}
}
}
void init(void) {
int i;
for(i=0;i<numSpheres;i++){
spheres[i][0] = (rand()%600)-300;//XPOS
spheres[i][1] = (rand()%600)-300;//YPOS
spheres[i][2] = rand()%400;//ZPOS
spheres[i][3] = rand()%20;//SIZE
spheres[i][4] = (rand()%20);//SPEED
}
MAXROT = 1.0/6.0*PI;
glClearColor (0.0, 0.0, 0.0, 0.0);
glEnable(GL_DEPTH_TEST);
glShadeModel(GL_SMOOTH);
pmodel = glmReadOBJ("city.obj");
if (!pmodel) fprintf(stderr,"Cannot parse vase.obj");
//glmUnitize(pmodel); // make model to fit in a unit cube
glmFacetNormals(pmodel); // generate normals - is this needed?
glmVertexNormals(pmodel, 90.0); // average joining normals - allow for hard edges.
tmodel = glmReadOBJ("tower.obj");
glmFacetNormals(tmodel); // generate normals - is this needed?
glmVertexNormals(tmodel, 90.0); // average joining normals - allow for hard edges.
readTop();
readFront();
readBack();
readRight();
readLeft();
readFire();
glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
glGenTextures(1, &front);
glGenTextures(1, &top);
glGenTextures(1, &left);
glGenTextures(1, &right);
glGenTextures(1, &back);
glGenTextures(1, &fire);
qsphere = gluNewQuadric();
}
/**** 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
/*
* main.c
* Author: Ian R Goodbody
* Function: Implements maze navigation
*/
void main(void)
{
WDTCTL = WDTPW | WDTHOLD; // Stop watchdog timer
initRangeFinders();
initMotors();
stop();
P1DIR |= LEFT_LED|RIGHT_LED;
P1OUT &= ~(LEFT_LED|RIGHT_LED);
int fBuffer[BUFFER_LN]; // Code probably unnecessary but it cannot hurt
int lBuffer[BUFFER_LN];
int rBuffer[BUFFER_LN];
int fMedian;
int lMedian;
int rMedian;
unsigned int lWheelSpeed = 4040;
unsigned int rWheelSpeed = 4000;
unsigned int runTime = 350;
int i;
fillBuffers(fBuffer, lBuffer, rBuffer);
while(1)
{
P1OUT &= ~(LEFT_LED|RIGHT_LED); // turn off LEDs
stop();
for (i = BUFFER_LN; i > 0; i--)
{
readFront(fBuffer);
waitMiliseconds(1);
readLeft(lBuffer);
waitMiliseconds(1);
readRight(rBuffer);
waitMiliseconds(1);
}
fMedian = median(fBuffer);
lMedian = median(lBuffer);
rMedian = median(rBuffer);
if(fMedian < 0x01F0) // Crash into wall test; ~2.5 inch threshold
{
if(lMedian < 0x01FF) // There is no wall remotely close to the left side,
{ // Initiate sweeping turn
lWheelSpeed = 2600;
rWheelSpeed = 5000;
}
else if(lMedian < 0x0266) // Getting too far from the wall start turning in
{
P1OUT |= LEFT_LED; // Red LED on, green off
P1OUT &= ~RIGHT_LED;
rWheelSpeed = 4270;
lWheelSpeed = 4040;
}
else if(lMedian > 0x02E4) // Getting too close to the wall start turning out
{
P1OUT |= RIGHT_LED; // Green LED on, red off
P1OUT &= ~LEFT_LED;
rWheelSpeed = 3900;
lWheelSpeed = 4040;
}
else // Acceptable distance from wall, cruise forward normally
{
//P1OUT |= RIGHT_LED;
P1OUT &= ~(RIGHT_LED|LEFT_LED);
rWheelSpeed = 4000;
lWheelSpeed = 4040;
}
setLeftWheel(lWheelSpeed, FORWARD);
setRightWheel(rWheelSpeed, FORWARD);
runTime = 350;
}
else
{
// About to run into a wall, initiate hard right turn
P1OUT |= RIGHT_LED|LEFT_LED;
setLeftWheel(5080,FORWARD);
setRightWheel(5260, BACKWARD);
runTime = 450;
}
go();
waitMiliseconds(runTime);
}
}
