Merge pull request #50 from Ernest314/development

Development

RobotC/6212/Autonomous Programs/Autonomous General.c

@@ -1,45 +1,160 @@
 #pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
-#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
-#pragma config(Sensor, S2,     sensor_1,       sensorNone)
-#pragma config(Sensor, S3,     sensor_2,       sensorNone)
-#pragma config(Sensor, S4,     sensor_3,       sensorNone)
-#pragma config(Motor,  motorA,          motor_claw,    tmotorNXT, PIDControl, encoder)
-#pragma config(Motor,  motorB,          motor_B,       tmotorNXT, openLoop)
-#pragma config(Motor,  motorC,          motor_C,       tmotorNXT, openLoop)
-#pragma config(Motor,  mtr_S1_C1_1,     motor_D,       tmotorTetrix, PIDControl, reversed)
-#pragma config(Motor,  mtr_S1_C1_2,     motor_E,       tmotorTetrix, PIDControl)
-#pragma config(Motor,  mtr_S1_C2_1,     motor_L,       tmotorTetrix, openLoop)
-#pragma config(Motor,  mtr_S1_C2_2,     motor_R,       tmotorTetrix, openLoop, reversed)
-//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
-
+#pragma config(Sensor, S2,     infrared,      sensorI2CCustom)
+#pragma config(Sensor, S3,     sensor_3,       sensorNone)
+#pragma config(Sensor, S4,     sensor_4,       sensorNone)
+#pragma config(Motor,  motorA,           ,             tmotorNXT, openLoop)
+#pragma config(Motor,  motorB,           ,             tmotorNXT, openLoop)
+#pragma config(Motor,  motorC,           ,             tmotorNXT, openLoop)
+#pragma config(Motor,  mtr_S1_C1_1,     motor_L,       tmotorTetrix, openLoop, reversed, encoder)
+#pragma config(Motor,  mtr_S1_C1_2,     motor_R,       tmotorTetrix, openLoop, encoder)
+#pragma config(Motor,  mtr_S1_C2_1,     motor_lift,    tmotorTetrix, openLoop)
+#pragma config(Motor,  mtr_S1_C2_2,     motor_G,       tmotorTetrix, openLoop)
 // Code generated by 'ROBOTC' configuration wizard
 #include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.
-#include "typedefs-6121.h"
-#include "global vars-6121.h"
-#include "enums-6121.h"
-#include "structs-6121.h"
-#include "low-level functions-6121.h"
-#include "high-level functions-6121.h"
-#include "subroutines-6121.h"
+#include "../Header Files/IR-driver.h"
+#include "../Header Files/enums-6121.h"
+#include "../Header Files/global vars-6121.h"
+#include "../Header Files/low-level functions-6121.h"
+#include "../Header Files/high-level functions-6121.h"
+#include "../Header Files/subroutines-6121.h"
 
 
 void initializeRobot()
 {
-	// Place code here to init servos to starting positions.
-	// Sensors are auto-config'ed and setup by RobotC. They may need to stabilize.
+	// Sensors are config'ed and setup by RobotC (need to stabalize).
 	return;
 }
 
 
+
 task main()
 {
-	initializeRobot();
-	waitForStart(); // Wait for the beginning of autonomous phase.
+	// The IR signal strengh in all 5 directions.
+
+	int IRdirA = 0;
+	int IRdirB = 0;
+	int IRdirC = 0;
+	int IRdirD = 0;
+	int IRdirE = 0;
+
+	typedef enum PegIR
+	{
+		PEG_I = 0,
+		PEG_II = 1,
+		PEG_III = 2,
+	};
+	PegIR isPeg = PEG_II;
+
+	waitForStart();
+ 	initializeRobot();
+
+
+
+	// For a better description of the below numbers,                   ________________________
+	// see the page in Engineering Notebook describing                  |+----------------------
+	// this program (it's labeled clearly).							    ||      Robot
+	const int forwardTimeA		= 58;	//								||	      |
+	const int turnTimeA			= 71;	//                              ||        | Fwd A
+	const int forwardTimeB		= 99;	//							    ||        |
+	const int backTimeA			= 90;	//                              ||   Turn A\
+                                       //								||	   	     \ Fwd B
+									   //								||
+	//Peg 1 Stuff
+	const int turnTimeIA		= 90;
+	const int forwardTimeIA		= 69;
+	const int turnTimeIB		= 99;
+	const int liftTimeIA		= 54;
+	const int forwardTimeIB		= 210;
+	const int liftTimeIB		= 59;
+	const int backTimeIA        = 250;
+	//Peg 2 Stuff
+	const int turnTimeIIA		= 90;
+	const int forwardTimeIIA	= 45;
+	const int turnTimeIIB		= 91;
+	const int liftTimeIIA		= 135;
+	const int forwardTimeIIB	= 220;
+	const int liftTimeIIB       = 40;
+	const int backTimeIIB       = 300;
+	const int liftTimeIIC		= 100;
+
+
+	//Peg 3 Stuff
+	const int turnTimeIIIA		= 87;
+	const int forwardTimeIIIA	= 175;
+	const int turnTimeIIIB		= 85;
+	const int liftTimeIIIA		= 51;
+	const int forwardTimeIIIB	= 180;
+	const int liftTimeIIIB		= 58;
+	const int backTimeIIIA      = 200;
+	const int turnTimeIIIC      = 175;
+	const int forwardTimeIIIC   = 200;
+
+	Time_Wait(100);
 
-	Move_Forward(100, 50);
+	Move_Forward	(forwardTimeA, g_AccurateMotorPower);
+	Turn_Left 		(turnTimeA, g_AccurateMotorPower, g_AccurateMotorPower);
+	Move_Forward	(forwardTimeB, g_AccurateMotorPower);
+
+	Time_Wait(100);
+	HTIRS2readAllACStrength(infrared, IRdirA, IRdirB, IRdirC, IRdirD, IRdirE);
+
+	if ( (IRdirA+IRdirB) > g_IRthreshold )
+		isPeg = PEG_III;
+	if ( (IRdirE+IRdirD) > g_IRthreshold )
+		isPeg = PEG_I;
+	if ( IRdirC > (IRdirA+IRdirB+IRdirD+IRdirE) )
+		isPeg = PEG_II;
+
+	switch (isPeg)
+	{
+		case PEG_I:
+			Move_Backward	(backTimeA, g_AccurateMotorPower);
+
+			//Turn_Right		(turnTimeIA, g_AccurateMotorPower, g_AccurateMotorPower);
+			//Move_Forward	(forwardTimeIA, g_AccurateMotorPower);
+			//Turn_Left		(turnTimeIB, g_AccurateMotorPower, g_AccurateMotorPower);
+
+			//Lift_Up			(liftTimeIA, g_AccurateMotorPower);
+			//Move_Forward	(forwardTimeIB, g_AccurateMotorPower);
+			//Lift_Down		(liftTimeIB, g_AccurateMotorPower);
+			//Move_Backward   (backTimeIA, g_AccurateMotorPower);
+			break;
+		case PEG_II:
+			Move_Backward	(backTimeA, g_AccurateMotorPower);
+
+			//Turn_Left		(turnTimeIIA, g_AccurateMotorPower, g_AccurateMotorPower);
+			//Move_Forward	(forwardTimeIIA, g_AccurateMotorPower);
+			//Turn_Right		(turnTimeIIB, g_AccurateMotorPower, g_AccurateMotorPower);
+
+			//Lift_Up			(liftTimeIIA, g_AccurateMotorPower);
+			//Move_Forward	(forwardTimeIIB, g_AccurateMotorPower);
+			//Lift_Down       (liftTimeIIB, g_AccurateMotorPower);
+			//Move_Backward   (backTimeIIB, g_AccurateMotorPower);
+			//Lift_Down		(liftTimeIIC, g_AccurateMotorPower);
+			break;
+		case PEG_III:
+			Move_Backward	(backTimeA, g_AccurateMotorPower);
+
+			//Turn_Left		(turnTimeIIIA, g_AccurateMotorPower, g_AccurateMotorPower);
+			//Move_Forward	(forwardTimeIIIA, g_AccurateMotorPower);
+			//Turn_Right		(turnTimeIIIB, g_AccurateMotorPower, g_AccurateMotorPower);
+
+			//Lift_Up			(liftTimeIIIA, g_AccurateMotorPower);
+			//Move_Forward	(forwardTimeIIIB, g_AccurateMotorPower);
+			//Lift_Down       (liftTimeIIIB, g_AccurateMotorPower);
+			//Move_Backward   (backTimeIIIA, g_AccurateMotorPower);
+			//Turn_Right      (turnTimeIIIC, g_AccurateMotorPower, g_AccurateMotorPower);
+			//Move_Forward    (forwardTimeIIIC, g_AccurateMotorPower);
+			break;
+	}
 
 	while (true)
 	{
-		Turn_Left(100, g_FullDrivePower, g_FullDrivePower);
+		PlaySoundFile("moo.rso");
+		while(bSoundActive == true)
+		{
+			Time_Wait(1);
+		}
 	}
+
 }