eh?

5 files changed, 274 insertions, 229 deletions

diff --git a/include/main.h b/include/main.h
index 6d36973..d1fcdb4 100644
--- a/include/main.h
+++ b/include/main.h
@@ -30,12 +30,13 @@ enum MOTOR_PORT_MAP {
 };

 

 enum IME_PORT_MAP {

-	IDRIVER,

+	IDRIVER = 0,

 	IDRIVEL,

 	IROTATER,

 	ILIFT1,

 	ILIFT2,

-	ICANNON3

+	ICANNONL,

+	ICANNONR

 };

 

 

diff --git a/src/auto.c b/src/auto.c
index 33204f9..3c8f2ef 100644
--- a/src/auto.c
+++ b/src/auto.c
@@ -1,6 +1,6 @@
 #include <main.h>

 

-#define TARGET_RPM 1970

+#define TARGET_SPEED 65

 

 static unsigned int time;

 

@@ -10,37 +10,32 @@ void autoDelay(unsigned int ms){
 }

 

 void autonomous(){

-	static int    trpm;

-	static double rpm;

-	static char cs;

+	static char speed = 0;

 

 	time = 15000;

-	cs = rpm = trpm = 0;

 

 	do{

 

-		cs+=cs<100?10:0;

+		speed+=10;

 

-		motorSet(CANNON1,cs);

-		motorSet(CANNON2,cs);

-		motorSet(CANNON3,cs);

-		motorSet(CANNON4,cs);

+		motorSet(CANNON1,speed);

+		motorSet(CANNON2,speed);

+		motorSet(CANNON3,speed);

+		motorSet(CANNON4,speed);

 

-		autoDelay(800);

+		autoDelay(200);

 

-		imeGetVelocity(ICANNON3,&trpm);

-		rpm=abs(trpm)/24.5L;

-		rpm*=25.0L;

+	}while(speed<TARGET_SPEED);

 

-	}while(rpm<TARGET_RPM);

+	autoDelay(800);

 

-	motorSet(LIFT1,127);

-	motorSet(LIFT2,127);

+	motorSet(LIFT1 ,127);

+	motorSet(LIFT2 ,127);

+	motorSet(INTAKE,127);

 

+	//while(1);

 	delay(time);

 

-	// Stop										4000ms remaining

-

 	motorStopAll();

 

 }

diff --git a/src/auton.c b/src/auton.c
new file mode 100644
index 0000000..8af90eb
--- /dev/null
+++ b/src/auton.c
@@ -0,0 +1,153 @@
+#include <main.h>
+
+extern Gyro gyro;
+
+#ifndef PI
+#define PI 3.14159265L
+#endif // PI
+
+#define RPM_DEFAULT_MAX		160				//	Max RPM of high-speed motor
+#define RPM_CANNON_MAX		4000			//	Max RPM of cannon (motor + gears)
+#define RPM_CANNON_INC		31.49606299L	//	Expected change in RPM per joystick increment
+
+#define RPM_THRESHOLD		100				//	A margin of error for matching the target RPM
+
+#define INC_FAULT			2				//	Boost given to motors when left and right don't match RPM
+#define INC_NORMAL			2				//	Used to match actual velocity to the target
+
+#define ROT_THRESHOLD		(PI/8)			//	Margin of error for auto-aiming
+#define ROT_SPEED			30				//	How fast to rotate the cannon for aiming (for linear aiming only)
+
+static double rpmTarget = 0;	// Contains the desired RPM
+
+static double rpmCannL = 0;		// Left side's RPM
+static double rpmCannR = 0;		// Right side's RPM
+static double rpmCannA = 0;		// Average of the two above
+
+static double rotCannon = 0;	// Cumulative rotation of the cannon in radians
+
+/**
+ * Calculates RPMs from cannon motors.
+ *
+ * There are two IMEs on the cannon, one per each side. This function converts the raw
+ * velocity reading from the motors and converts it to rotations per minute (RPM).
+ * Results are stored in global variables defined above, as well as an average of the
+ * two.
+ */
+
+void cannUpdate(void){
+	int cl,cr,rc;
+
+	/*
+	 *	Read raw velocity values.
+	*/
+
+	imeGetVelocity(ICANNONL,&cl);
+	imeGetVelocity(ICANNONR,&cr);
+
+	/*
+	 *	Divide by a divisor given in API.h and multiply the result by the cannon's gear ratio.
+	*/
+
+	rpmCannL = cl / 24.5L * 25.0L;
+	rpmCannR = cr / 24.5L * 25.0L;
+
+	/*
+	 *	Calculate an average of the two results from above.
+	*/
+
+	rpmCannA = (rpmCannL + rpmCannR) / 2;
+
+	/*
+	 *	Find the cumulative rotation of the cannon for auto-aiming.
+	*/
+
+	imeGet(IROTATER,&rc);
+
+	rotCannon = rc / 627.2L * 5;
+	rotCannon *= 2*PI;				// convert to radians
+
+}
+
+/**
+ * Sets a target RPM for the cannon.
+ *
+ * This calculates a target RPM by simply multiplying the current position of a jostick axis
+ * by a predetermined increment that is 1/127th of the highest possible RPM of the cannon.
+ *
+ * @param a value returned from a call to joystickGetAnalog()
+ */
+
+void cannTarget(char jpos){
+	rpmTarget = jpos * RPM_CANNON_INC;
+}
+
+/**
+ * Runs/Updates the actual speed of the cannon motors.
+ *
+ * Speeds for each side of the cannon are kept inside the scope of this function. Two checks
+ * are made before applying the motor speeds. The first one will increase the speed of one
+ * side of the cannon if the RPMs of the two sides aren't reasonably equal. The second check
+ * will bring the overall speeds up or down depending on where the actual average RPM is
+ * compared to the target. Following these checks is the setting of the motor speeds.
+ */
+
+void cannRun(void){
+	static char speedCannL = 0;
+	static char speedCannR = 0;
+
+	/*
+	 *	Make sure cannon motors are up to about the same speed (as in RPM).
+	*/
+
+		 if(rpmCannR < rpmTarget + RPM_THRESHOLD && rpmCannL < rpmCannR - RPM_THRESHOLD) speedCannL += INC_FAULT;
+	else if(rpmCannL < rpmTarget + RPM_THRESHOLD &&	rpmCannR < rpmCannL - RPM_THRESHOLD) speedCannR += INC_FAULT;
+
+	/*
+	 *	Adjust motor velocities to match the target.
+	*/
+
+	if(rpmTarget > rpmCannA + RPM_THRESHOLD){
+		speedCannL += INC_NORMAL;
+		speedCannR += INC_NORMAL;
+	}else if(rpmTarget < rpmCannA - RPM_THRESHOLD){
+		speedCannL -= INC_NORMAL;
+		speedCannR -= INC_NORMAL;
+	}
+
+	/*
+	 *	Apply the calculated motor speeds.
+	*/
+
+	motorSet(CANNON1,speedCannL);
+	motorSet(CANNON2,speedCannR);
+	motorSet(CANNON3,speedCannL);
+	motorSet(CANNON4,speedCannR);
+}
+
+/**
+ * Aims the cannon towards a specified location(?)
+ *
+ * ... . Also sets the motor speed.
+ *
+ * @param a rotation to base calculations of off (preferably that of the robot's body)
+ */
+
+void cannAim(double r){
+	static char speed = 0;
+
+	/*
+	 *	Do simple linear adjustments to have okay-ish aim. Once self-location is accomplished
+	 *	a more accurate function could be used to adjust aim.
+	*/
+
+		 if(rotCannon > r + ROT_THRESHOLD) speed =  ROT_SPEED;
+	else if(rotCannon < r - ROT_THRESHOLD) speed = -ROT_SPEED;
+	else								   speed =  0;
+
+	/*
+	 *	Apply the resulting speed to the motor.
+	*/
+
+	motorSet(ROTATER,speed);
+}
diff --git a/src/init.c b/src/init.c
index 3ffa429..a59dcba 100644
--- a/src/init.c
+++ b/src/init.c
@@ -6,6 +6,9 @@ unsigned int imeCount;
 void initializeIO(){

 	pinMode(ANALOG_PORT(1),INPUT_ANALOG);	// Power expander status port

 	pinMode(1,OUTPUT);						// LED

+	pinMode(2,INPUT);

+	pinMode(3,INPUT);

+	pinMode(4,INPUT);

 }

 

 void initialize(){

@@ -16,26 +19,14 @@ void initialize(){
 	lcdClear(uart1);

 	lcdSetBacklight(uart1,1);

 

-	lcdInit(uart2);

-	lcdClear(uart2);

-	lcdSetBacklight(uart2,1);

-

 	//	Setup sensors.

 

 	imeCount = imeInitializeAll();

 	gyro=gyroInit(2,0);

 

 	lcdPrint(uart1,1,"%u IMEs :)",imeCount);

-

-	lcdPrint(uart2,1,"ERR: Dark theme ");

-	lcdPrint(uart2,2,"     not found! ");

-

-	delay(2000);

-

+	delay(1000);

 	lcdPrint(uart1,1,"ready...");

-	lcdPrint(uart2,1,"...yeah.        ");

-	lcdPrint(uart2,2,"                ");

-

 	delay(1000);

 

 }

diff --git a/src/opcontrol.c b/src/opcontrol.c
index d4d50a2..8e410fa 100644
--- a/src/opcontrol.c
+++ b/src/opcontrol.c
@@ -2,133 +2,60 @@
 #include <stdint.h>

 #include <math.h>

 

-extern Gyro gyro;	// Originally defined in init.c

-

-/*

- *	These keep track of the current LCD 'window' and the number of 'windows' that exist.

- */

-

-static   signed int ctty=0;

-const  unsigned int mtty=4;

-

-static double imeDriveLeft;

-static double imeDriveRight;

-static double imeRotater;

-

-static double gyroRotation;

-

-/*

- *	This runs as a separate task to update the LCD.

- */

-

-void operatorLCD(void *unused){

-	static int ime;

-

-	while(1){

-

-		/*

-		 *	Display information according to the current 'window'.

-		 */

-

-		switch(ctty){

-		default:

-

-		case 0:	//	Welcome screen

-

-			lcdSetText(uart1,1,"   JOHN  CENA   ");

-			lcdPrint(uart1,2,"  ===========%3d",motorGet(CANNON1));

-			break;

-

-		case 1:	//	Battery levels in volts

-

-			lcdPrint(uart1,1,"MAIN: %0.3f",(float)(powerLevelMain()/1000.0f));

-			lcdPrint(uart1,2,"EXP : %0.3f",(float)(analogRead(1)   / 280.0f));

-			break;

-

-		case 2:	//	Gyroscope readings

-

-			imeGet(IROTATER,&ime);

+#define PI 3.1415926535L

 

-			double rot = ime / 3.0L / 627.2L * 10;

-

-			rot = floor(rot) / 10;

-

-			lcdPrint(uart1,1,"Gyro: %d",gyroGet(gyro));

-			lcdPrint(uart1,2,"Rot.: %.1lf",rot);

-

-			break;

+extern Gyro gyro;	// Originally defined in init.c

 

-		case 3:

+static double Vl,	// left wheel velocity

+			  Vr,	// right wheel velocity

+			  Vc,	// center/average velocity

+			  Va,	// angular velocity

+			  Vra;	// velocity ratio

 

-			lcdPrint(uart1,1,"%.3lf",imeDriveLeft);

-			lcdPrint(uart1,2,"%.3lf",imeDriveRight);

+static double R;	// radius of current circular path

 

-			break;

-		}

+static double Px=0,	// X position

+			  Py=0;	// Y position

 

-		delay(500);

-	}

-}

-

-void operatorPoll(){

-	static int idl,idr,ir,gr;

+void operatorLCD(void *start){

 	while(1){

 

-		/*

-		 *	Read in rotations of motors and the current angle of the robot.

-		 */

-

-		imeGet(DRIVEL,&idl);			// Get reading

-		imeDriveLeft  = idl / 627.0L;	// Convert to wheel rotations

-		imeDriveLeft *=  8.64L;			// Convert to distance in inches

-

-		imeGet(DRIVER,&idr);			// Get reading

-		imeDriveRight  = idr / 627.0L;	// Convert to wheel rotations

-		imeDriveRight *=  8.64L;		// Convert to distance in inches

-

-		imeGet(ROTATER,&ir);

-		imeRotater = ir / 627.0L;

-

-		gyroRotation = gyroGet(gyro);

+		lcdPrint(uart1,1,"%.3lf, %.3lf",R,Va);

 

 		delay(100);

 	}

 }

 

-void operatorControl(){

+static unsigned int START;

 

-	static bool selfAim = false;

+void operatorControl(){

 

 	static char uiinc = 0;	// See below

+	static char in=0,lift;	// Used to set the multiple cannon motors to the same joy-stick reading.

 

-	static char in,lift;	// Used to set the multiple cannon motors to the same joy-stick reading.

+	static char joy;

 

-	taskCreate(operatorLCD ,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT);

-	taskCreate(operatorPoll,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT);

+	/*

+	 *	Start other tasks.

+	*/

+

+	START = ((!digitalRead(3)) | (!digitalRead(4)<<1)) + 1;

+	taskCreate(operatorLCD ,TASK_DEFAULT_STACK_SIZE,&START,TASK_PRIORITY_DEFAULT);

 

 	while(1){

 

 		//	Set the drive motors speeds.

 

-		motorSet(DRIVEL,joystickGetAnalog(1,3));

-		motorSet(DRIVER,joystickGetAnalog(1,2));

+		joy=joystickGetAnalog(1,3);

+		if(joy < 10 && joy > -10)joy=0;

+		motorSet(DRIVEL,joy);

+		joy=joystickGetAnalog(1,2);

+		if(joy < 10 && joy > -10)joy=0;

+		motorSet(DRIVER,joy);

 

 		//	Set the rotating motor speed.

 

-		if(!selfAim){

-

-			motorSet(ROTATER,-joystickGetAnalog(2,1)/2);

-

-		}else{

-

-			//static int gc=0,gl=0;

-

-			//gl = gc;

-			//gc = gyroGet(gyro);

-

-		}

-

-		//	Set the cannon's speed.

+		motorSet(ROTATER,-joystickGetAnalog(2,1)/4);

 

 		in=joystickGetAnalog(2,3);

 

@@ -140,14 +67,14 @@ void operatorControl(){
 		// Set the intake's speed.

 

 		motorSet(INTAKE,joystickGetDigital(1,6,JOY_UP  )?  127 :

-				joystickGetDigital(1,6,JOY_DOWN)? -127 :

+						joystickGetDigital(1,6,JOY_DOWN)? -127 :

 						0 );

 

 		// Set the lift's speed.

 

 		lift=joystickGetDigital(2,6,JOY_UP  )?  127 :

-				joystickGetDigital(2,6,JOY_DOWN)? -127 :

-						0 ;

+			 joystickGetDigital(2,6,JOY_DOWN)? -127 :

+			 0 ;

 

 		motorSet(LIFT1,lift);

 		motorSet(LIFT2,lift);

@@ -159,130 +86,108 @@ void operatorControl(){
 		if(++uiinc==20){	// Equates to every 200ms

 			uiinc=0;

 

-			// Goto next 'window'.

-

-			if(joystickGetDigital(1,7,JOY_UP) ||

-					joystickGetDigital(2,7,JOY_UP) ){

-				if(++ctty==mtty)ctty=0;

-			}

-

-			// Goto previous 'window'.

-

-			if(joystickGetDigital(1,7,JOY_DOWN) ||

-					joystickGetDigital(2,7,JOY_DOWN) ){

-				if(--ctty==-1)ctty=mtty-1;

-			}

-

 			// Run autonomous (for testing wo/ competition switch).

 

 			if(joystickGetDigital(1,7,JOY_RIGHT))

 				autonomous();

 

-			// Goto test area.

+			// Test

 

 			if(joystickGetDigital(1,7,JOY_LEFT)){

-				static double target = 0;

 

-				if(!target) target = (imeDriveLeft+imeDriveRight) / 2 + 13;

+			}

 

-				motorSet(DRIVEL,60);

-				motorSet(DRIVER,60);

+		}

 

-				while((imeDriveLeft+imeDriveRight) / 2 < target)

-					delay(10);

+		delay(10);	// Short delay to allow task switching

 

-				motorSet(DRIVEL,0);

-				motorSet(DRIVER,0);

+		/*

+		 *	Get the velocity of the two (IME'd) drive motors.

+		*/

 

-			}

+		static int vl,vr;

 

-		}

+		imeGetVelocity(1,&vl);

+		imeGetVelocity(0,&vr);

 

-		delay(10);	// Short delay to allow task switching.

-	}

-}

+		/*

+		 *	Convert the raw reading to rotations a minute, then inches per minute, then inches

+		 *	per second.

+		*/

 

+		Vl =  vl /* RPM Divisor         */ / 39.2L

+				 /* Wheel Circumference */ * 8

+				 /* Minutes to seconds  */ / 60;

 

-//Totally Theoretical PseudoCode for AutoShoot

-//Based on IME data:

-double lVel;//=velocity of left side

-double rVel;//=velocity of right side

-double cVel;//=velocity of center (calculated below)

-double aVel;//=angular velocity (calculated below)

+		Vr = -vr / 39.2L * 8 / 60;	// Right wheel IME is inverted

 

+		Vc = (Vl + Vr) / 2;			// Average/Center velocity

 

-//Used to find rectangular vectors * double aVel=angular velocity of robot

-//Used to store the rectangular vectors:

-double xVel1;

-double xVel2;

-double yVel1;

-double yVel2;

+		/*

+		 *	Round down the results to the nearest inch, and enforce a 2 in/s threshold.

+		*/

 

+		Vl = floor(Vl);

+		Vr = floor(Vr);

 

-//Final Position Vectors, robot and target

-double xPos;

-double yPos;

-double xTarget;

-double yTarget;

+		if(Vl < 2 && Vl > -2) Vl = 0;

+		if(Vr < 2 && Vr > -2) Vr = 0;

 

+		/*

+		 *	Calculate the ratio of the higher velocity to the lowest, for determining the

+		 *	radius of the circle the robot is forming.

+		*/

 

-//time difference variables

-double time1=0;

-double time2=0;

+		if(((!Vr) & (Vl!=0)) ||

+		   ((!Vl) & (Vr!=0)) ){

 

-int start;//1 is right blue, 2 is left blue, 3 is right blue, 4 is left blue

+			/*

+			 *	One wheel isn't moving, the radius is the distance between the wheels.

+			*/

 

+			R = 15;

+			goto CONT;

 

-//Vector Assignments:

-void Vectors(){

-	if(start==1){xPos=37;yPos=138;xTarget=137;yTarget=7;}

-	if(start==2){xPos=13;yPos=114;xTarget=137;yTarget=7;}

-	if(start==3){xPos=37;yPos=13;xTarget=137;yTarget=137;}

-	if(start==4){xPos=13;yPos=37;xTarget=137;yTarget=137;}

-	while(1){//something in the brackets

+		}else if((Vr > Vl) & (Vl > 0) ||	// Curve to forwards right

+				 (Vl > Vr) & (Vl < 0) ){	// Curve to backwards right

 

-		imeGetVelocity(DRIVEL,&lVel);//get reading of left velocity

-		lVel=lVel/24.5*8.64;

+			Vra = Vr / Vl;

 

-		imeGetVelocity(DRIVER,&rVel);//get reading of right velocity

-		rVel=rVel/24.5*8.64;

+		}else if((Vl > Vr) & (Vr > 0) ||	// Curve to forwards left

+				 (Vr > Vl) & (Vr < 0) ){	// Curve to backwards left

 

-		int i=0;//counter for use of alternating variables

-		i++;

-		if(i==1){//just used the first time

-			time1=milis();

-		}

-		cVel=(lVel+rVel)/2;//calculates the (c)enter of the robot's velocity

+			Vra = Vl / Vr;

 

-		if(lVel>rVel){

-			aVel=cVel/(16*lVel/(rVel-lVel));

-		}

+		}else Vra = 0;						// No movement?

 

-		if(rVel>lVel){

-			aVel=cVel/(16*rVel/(lVel-rVel));

-		}

+		if(Vra<0) Vra *= -1;				// get absolute value of the ratio

 

-		else{

-			aVel=0;

+		/*

+		 *	"Plug-n'-chug" for a radius, assuming that the radius will increase by 7.5 (the

+		 *	halfway between the wheels on the robot) when multiplied by the ratio.

+		*/

 

-			if(i%2==0){

-				xVel1=cos(aVel)*cVel;//every 2 it uses these variables

-				yVel1=sin(aVel)*cVel;

-			}

+		for(R = 0; R < 200; R++){

 

-			else{

-				xVel2=cos(aVel)*cVel;//otherwise it uses these

-				yVel2=sin(aVel)*cVel;

-			}

+			if(R * Vra > R + 7 &&		// Allow a one inch margin of error

+			   R * Vra < R + 8)break;	//

+		}

 

-			time2=milis();//records the time before calculating

+CONT:

 

-			xPos+=((xVel1+xVel2)/2)*(time2-time1);//finds the area of the x curve using trapezoidal approx.

-			yPos+=((yVel1+yVel2)/2)*(time2-time1);//finds the area of the y curve using trapezoidal approx.

+		/*

+		 *	Calculate the anglular velocity of the robot.

+		*/

 

-			delay(20);

-		}

-		time1=milis();//records time between calculations for next time

-	}//end of while

-}//end of Vectors

+		Va = Vc / R;

 

+		/*

+		 *	Determine the increase in X and Y position based on the angular velocity and the

+		 *	average total movement.

+		*/

+

+		Px += cos(Va) * Vc * .01;	// Convert per second velocity to per 10ms, as the motors

+		Py += sin(Va) * Vc * .01;	// have only been at this velocity for that time (above).

+

+	}

+}