コード例 #1
0
ファイル: OnOffControl.c プロジェクト: tac2155/Mechatronics
void	Mode1(void)
{
	while(1 != 2)
	{
		waitPress();
		if(green)
		{
			break;
		}
		mainTran	= 1;
		waitPress();
		if(green)
		{
			break;
		}
		mainTran	= 0;
	}
}
コード例 #2
0
ファイル: OnOffControl.c プロジェクト: tac2155/Mechatronics
void	Mode3(void)
{
	while(1 != 2)						// infinte loop
	{
		error1 	= 10;
		error2	= 10;
		error3	= 2;
		waitPress();
		Count	= AtoD();
		mainTran	= 1;
		while(!engaged)
		{
			longTimer();
			error1--;
			if(error1 == 0)
			{
				error();
			}
		}
		secTran	 = 1;
		switchDelay();
		mainTran = 0;
		while(Count > 0)
		{
			longTimer(); 				// 1s delay loop
			if(!engaged)
			{
				error3--;
				if(error3 == 0)
				{
					error();
				}
				mainTran	= 1;
				SwitchDelay();
				mainTran	= 0;
			}
			Count--;
		}
		secTran	= 0;
		while(engaged)
		{
			longTimer();
			error2--;
			if(error2 == 0)
			{
				error();
			}
		}
	}
}
コード例 #3
0
ファイル: OnOffControl.c プロジェクト: tac2155/Mechatronics
void	Mode2(void)
{
	while(1 != 2)						// infinite loop
	{
		waitPress();					// wait for button press
		Count	= AtoD();				// get timer value
		mainTran	= 1;				// turn on main transistor
		while(Count > 0)
		{
			if (redButton)
			{
				Count = AtoD();
			}
			longTimer();				// 1s delay loop
			Count--;
		}
		mainTran	= 0;
	}
}
コード例 #4
0
task main()
{
	displayLCDCenteredString(0, "Autonomus");
	displayLCDCenteredString(1, "Recorder V1.0");
	wait1Msec(2000);
	clearLCD();
	displayLCDCenteredString(0, "Start");
	displayLCDCenteredString(1, "\/");
	waitPress(centerButton);
	waitRelease();
	clearLCD();
	displayLCDCenteredString(0, "Time Remaining");
	ClearTimer(T1);
	int i = -1;
	while(time1[T1] < 1000*AUTONLENGTH)
	{
		i++;
		clearLCDLine(1);
		timeRemaining = AUTONLENGTH-(time1[T1]*0.001);
		sprintf(display, "%1.1f%c", timeRemaining);
		displayLCDCenteredString(1, display);
		baseEncoderReset();
		liftArmEncoderReset();
		driverControl(false);
		temp[tempRightSpeed] = motor[backRight]; //Right Speed
		temp[tempLeftSpeed] = motor[backLeft]; //Left Speed
		temp[tempRightDistance] = nMotorEncoder[backRight]; //Right Distance
		temp[tempLeftDistance] = nMotorEncoder[backLeft]; //Left Distance
		temp[tempHangingArmDistance] = SensorValue[liftArmEncoder]; //Lift Encoder
		temp[tempHangingArmSpeed] = motor[liftTopRight]; //Lift Speed
		temp[tempIntakeSpeed] = motor[intakeRight]; //Intake Speed
		if(abs(temp[0] - temp[2]) < 11) //Straight
		{
			if(temp[0] > 0) //Forward
			{
				data[i][dataRightSpeed] = (temp[tempRightSpeed] + temp[tempLeftSpeed])/2; //Right Speed
				data[i][dataRightDistance] = (temp[tempRightDistance] + temp[tempLeftDistance])/2; //Right Distance
				data[i][dataLeftSpeed] = data[i][dataRightSpeed]; //Left Speed
				data[i][dataLeftDistance] = data[i][dataRightDistance]; //Left Distance
				data[i][dataMovementCode] = codeForward; //Forward Code
			}
			else if(temp[0] < 0) //Backward
			{
				data[i][dataRightSpeed] = (temp[tempRightSpeed] + temp[tempLeftSpeed])/2; //Right Speed
				data[i][dataRightDistance] = (temp[tempRightDistance] + temp[tempLeftDistance])/2; //Right Distance
				data[i][dataLeftSpeed] = data[i][dataRightSpeed]; //Left Speed
				data[i][dataLeftDistance] = data[i][dataRightDistance]; //Left Distance
				data[i][dataMovementCode] = codeBackward; //Backward Code
			}
			else //Not moving
			{
				data[i][dataRightSpeed] = 0;
				data[i][dataRightDistance] = temp[tempRightDistance];
				data[i][dataLeftSpeed] = 0;
				data[i][dataLeftDistance] = temp[tempLeftDistance];
				data[i][dataMovementCode] = codeWait; //Wait Code
			}
		}
		else if((temp[tempRightSpeed] < temp[tempLeftSpeed] && temp[tempRightSpeed] > 0) || (temp[tempRightSpeed] > temp[tempLeftSpeed] && temp[tempRightSpeed] < 0)) //Right Turn
		{
			data[i][dataRightSpeed] = temp[tempRightSpeed];
			data[i][dataLeftSpeed] = temp[tempLeftSpeed];
			data[i][dataRightDistance] = temp[tempRightDistance];
			data[i][dataLeftDistance] = temp[tempLeftDistance];
			data[i][dataMovementCode] = codeRightTurn;
		}
		else if((temp[tempRightSpeed] > temp[tempLeftSpeed] && temp[tempRightSpeed] > 0) || (temp[tempRightSpeed] < temp[tempLeftSpeed] && temp[tempRightSpeed] < 0)) //Left Turn
		{
			data[i][dataRightSpeed] = temp[tempRightSpeed];
			data[i][dataLeftSpeed] = temp[tempLeftSpeed];
			data[i][dataRightDistance] = temp[tempRightDistance];
			data[i][dataLeftDistance] = temp[tempLeftDistance];
			data[i][dataMovementCode] = codeLeftTurn;
		}
		if(temp[tempHangingArmSpeed] != 0)
		{
			data[i][dataMovementCode] += codeHangingArm;
			if(temp[tempHangingArmSpeed] > 0)
			{
				data[i][dataMovementCode] += codePlus;
			}
			else
			{
				data[i][dataMovementCode] += codeMinus;
			}
		}
		if(tempIntakeSpeed != 0)
		{
			data[i][dataMovementCode] += codeIntake;
			if(temp[tempIntakeSpeed] > 0)
			{
				data[i][dataMovementCode] += codePlus;
			}
			else
			{
				data[i][dataMovementCode] += codeMinus;
			}
		}
		if(temp[tempBallLift] != 0)
		{
			data[i][dataMovementCode] += codeBallLift;
			data[i][dataMovementCode] += codePlus;
		}
		data[i][dataHangingArmDistance] = temp[tempHangingArmDistance];
		if(!firstRun)
		{
			if((data[i][dataMovementCode]/10) == (data[i-1][dataMovementCode]/10))
			{
				i--;
				data[i][dataMovementCode] = data[i+1][dataMovementCode];
				data[i][dataRightSpeed] = (data[i+1][dataRightSpeed] + data[i][dataRightSpeed]) / 2;
				data[i][dataLeftSpeed] = (data[i+1][dataLeftSpeed] + data[i][dataLeftSpeed]) / 2;
			}
			i++;
			data[i][dataRightDistance] += data[i+1][dataRightDistance];
			data[i][dataLeftDistance] += data[i+1][dataLeftDistance];
			data[i][dataHangingArmDistance] += data[i+1][dataHangingArmDistance];
		}
		else
		{
			firstRun = false;
		}
	}

	for(int i=1;i<AUTONLENGTH*20;i++)
	{
		data[i][dataRightDistance] = 127;
		data[i][dataMovementCode] = codeForward+1;
		data[i][dataRightSpeed] = 63;
		if((data[i][dataMovementCode]/10)*10 == codeForward)
		{
			writeDebugStreamLine("");
			writeDebugStream("forward(");
			writeDebugStream("%1.0f%c", data[i][dataRightSpeed]);
			writeDebugStream(", ");
			writeDebugStream("%1.0f%c", data[i][dataRightDistance]);
			writeDebugStream(");");
		}
		else if((data[i][dataMovementCode]/10)*10 == codeBackward)
		{
			writeDebugStreamLine("");
			writeDebugStream("backward(");
			writeDebugStream("%1.0f%c", data[i][dataRightSpeed]);
			writeDebugStream(", ");
			writeDebugStream("%1.0f%c", data[i][dataRightDistance]);
			writeDebugStream(");");
		}
		else if((data[i][dataMovementCode]/10)*10 == codeLeftTurn)
		{
			writeDebugStreamLine("");
			writeDebugStream("leftTurn(");
			writeDebugStream("%1.0f%c", data[i][dataRightSpeed]);
			writeDebugStream(", ");
			writeDebugStream("%1.0f%c", data[i][dataLeftSpeed]);
			writeDebugStream(", ");
			writeDebugStream("%1.0f%c", data[i][dataRightDistance]);
			writeDebugStream(", ");
			writeDebugStream("%1.0f%c", data[i][dataLeftDistance]);
			writeDebugStream(");");
		}
		else if((data[i][dataMovementCode]/10)*10 == codeRightTurn)
		{
			writeDebugStreamLine("");
			writeDebugStream("rightTurn(");
			writeDebugStream("%1.0f%c", data[i][dataRightSpeed]);
			writeDebugStream(", ");
			writeDebugStream("%1.0f%c", data[i][dataLeftSpeed]);
			writeDebugStream(", ");
			writeDebugStream("%1.0f%c", data[i][dataRightDistance]);
			writeDebugStream(", ");
			writeDebugStream("%1.0f%c", data[i][dataLeftDistance]);
			writeDebugStream(");");
		}
	}
}