IRVision.c

#pragma config(Hubs,  S1, HTServo,  none,     none,     none)
#pragma config(Sensor, S2,     leftVal,        sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S3,     rightVal,       sensorHiTechnicIRSeeker1200)
#pragma config(Servo,  srvo_S1_C1_1,    leftIR,               tServoStandard)
#pragma config(Servo,  srvo_S1_C1_2,    rightIR,              tServoStandard)
#pragma config(Servo,  srvo_S1_C1_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C1_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C1_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C1_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.

const int leftIRZero = 4;
const int leftIR180 = 237;
const int rightIRZero = 6;
const int rightIR180 = 243;

float rightTotal = 0;
float dist = 11.5;
float leftTotal = 0;
float leftAverage = 0;
int rightNumOfReadings = 0;
float rightAverage = 0;
int leftNumOfReadings = 0;
float leftAngle = 0;
float rightAngle = 0;
float rightcurrent = 0;
float leftcurrent = 0;
int rightcount = 0;
int leftcount = 0;
bool left5 = false;
bool right5 = false;
double x;
double y;
double leftsense;

task Vision();
task PostCalc();
task main()
{
	forward(10);

	//turn 90 degrees
	//not sure how we start

	servo[leftIR] = leftIRZero;
	servoChangeRate[leftIR] = 4;
	servoChangeRate[rightIR] = 4;
	servo[rightIR] = rightIRZero;
	wait1Msec(1000);
	StartTask(Vision);
	float target = 10;
  while (rightNumOfReadings < target || leftNumOfReadings < target)
  {
  	eraseDisplay();
  	nxtDisplayTextLine(4,"Angle 1: %f \n Angle 2: %f",leftAngle,rightAngle);
  }
  while(true)
  {
  	eraseDisplay();
  	nxtDisplayTextLine(4,"Angle 1: %f \n Angle 2: %f",leftAngle,rightAngle);
  }
}
double Tangent(double a){
	double tan = sin((PI/180)*a)/cos((PI/180)*a);
	return tan;
}
void forward(float distance) {//forward function to pass in a length in inches and then it goes that for
const float CHANGE = 2;
float totalTraveled = 0;
float encoderTarget = (169.92*distance) - 55.875; //calculate the encoder target
nMotorEncoder[rightDrive] = 0;
nMotorEncoder[leftDrive]  = 0;
float leftEncoder;
float rightEncoder;
float leftPower  = 50;
float rightPower = 50;
while(abs(totalTraveled) < abs(encoderTarget))
{
	leftEncoder  = nMotorEncoder[leftDrive];
	rightEncoder = nMotorEncoder[rightDrive];
	if(leftEncoder > rightEncoder)//changes based on which one has traveled farther
	{
		leftPower  -= CHANGE;
		rightPower += CHANGE;
	}
	else if(leftEncoder < rightEncoder)//same thing
	{
		leftPower  += CHANGE;
		rightPower -= CHANGE;
	}
	motor[leftDrive]  = leftPower;
	motor[rightDrive] = rightPower;
	totalTraveled += (leftEncoder + rightEncoder)/ 2.0;
	nMotorEncoder[rightDrive] = 0;
	nMotorEncoder[leftDrive]  = 0;
	ClearTimer(T1);
	while(time1[T1] < 200) {
		leftEncoder  = nMotorEncoder[leftDrive];
		rightEncoder = nMotorEncoder[rightDrive];
		if(!((abs(totalTraveled + (leftEncoder + rightEncoder)/ 2.0)/* average*/)/*adds the distance traveled*/ < abs(encoderTarget)))
		{//while all that is not less than the encoder target
			totalTraveled += (nMotorEncoder[leftDrive] + nMotorEncoder[rightDrive])/ 2.0;// add the aveage of the encoders
			break;//exit
		}
	}
}

motor[leftDrive]  = 0;
motor[rightDrive] = 0;
nMotorEncoder[rightDrive] = 0;
nMotorEncoder[leftDrive]  = 0;
}
task leftCalc() {//calc when 5 and 6
	while(true)
	{
		leftsense = SensorValue[leftVal];
		if(SensorValue[leftVal] == 5)
		{

			leftcount++;
			leftTotal += servo[leftIR];
			leftNumOfReadings ++;
			leftAverage = leftTotal / leftNumOfReadings;
			leftAngle = (leftAverage - leftIRZero) / (leftIR180 / 180.0);
			if((leftcount ==4) &&(left5 == false)){
				left5 = true;
				leftcurrent = leftAverage;
				leftcount = 0;
				leftNumOfReadings = 0;
				leftTotal = 0;
				StartTask(PostCalc);
			}
		}
	}
}
task rightCalc() {//still need to do some editing with this and left
	while(true) {
		if(SensorValue[rightVal] == 5)
		{
			rightcount++;
			rightTotal += servo[rightIR];
			rightNumOfReadings ++;
			rightAverage = rightTotal /(float) rightNumOfReadings;
			rightAngle = (rightAverage - rightIRZero) / (rightIR180 / 180.0);
			if((rightcount == 4) && (right5 == false)){
				right5 = true;
				rightcurrent = rightAverage;
				rightcount = 0;
				rightTotal = 0;
				rightNumOfReadings = 0;
			}
		}
	}
}
task PostCalc(){
	x= (-1*dist)*Tangent(leftcurrent);
	y= (-1*dist)*Tangent(rightcurrent);
	//add stuff here

}
task Calc() {
	StartTask(leftCalc);
	StartTask(rightCalc);
}
task act(){
	//while(the sensors are getting no readings)
	if(SensorValue[rightVal] == 5){
		forward(5);
		wait1Msec(5000);
	}
	else if(SensorValue[rightVal] == 4)
	{

		motor[rightDrive] = 2
		wait1Msec(500);
		motor[rightdirve] = 0;
		//turn sligtly
		forward(5);
		wait1Msec(1500);

	}
	else if(SensorValue[rightVal] == 3)
	{
		motor[rightDrive] = 8;
		wait1Msec(500);
		motor[rightdirve] = 0;
		forward(5);
		wait1Msec(1500);
	}
	else if (SensorValue[rightVal] == 2)
	{
		motor[rightDrive] = 13;
		wait1Msec(500);
		motor[rightdirve] = 0;
		forward(5);
		wait1Msec(1500);

	}
	else if (SensorValue[rightVal] == 1)
	{
		//go back and adjust
	}
	else if(SensorValue[rightVal])

}
task Vision() {
	StartTask(Calc);
	bool leftIRGoingLeft = false;
	bool rightIRGoingLeft = false;
	while(true)
	{
		if(servo[leftIR] == leftIR180)
		{
			leftIRGoingLeft = true;
		}
		else if(servo[leftIR] == leftIRZero)
		{
			leftIRGoingLeft = false;
		}
		if(servo[rightIR] == rightIR180)
		{
			rightIRGoingLeft = true;
		}
		else if(servo[rightIR] == rightIRZero)
		{
			rightIRGoingLeft = false;
		}
		if(leftIRGoingLeft)
		{
			servo[leftIR] = servo[rightIR] - 1.0;
		}
		else
		{
			servo[leftIR] = servo[leftIR] + 1.0;
		}
		if(rightIRGoingLeft)
		{
			servo[rightIR] = servo[rightIR] - 1.0;
		}
		else
		{
			servo[rightIR] = servo[rightIR] + 1.0;
		}
		wait1Msec(10);
	}
}

/* TODO
possible turn function
light sensor movement
multiplexer working in code
*/