--- /dev/null
+#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);
+}
#include <stdint.h>\r
#include <math.h>\r
\r
-extern Gyro gyro; // Originally defined in init.c\r
-\r
-/*\r
- * These keep track of the current LCD 'window' and the number of 'windows' that exist.\r
- */\r
-\r
-static signed int ctty=0;\r
-const unsigned int mtty=4;\r
-\r
-static double imeDriveLeft;\r
-static double imeDriveRight;\r
-static double imeRotater;\r
-\r
-static double gyroRotation;\r
-\r
-/*\r
- * This runs as a separate task to update the LCD.\r
- */\r
-\r
-void operatorLCD(void *unused){\r
- static int ime;\r
-\r
- while(1){\r
-\r
- /*\r
- * Display information according to the current 'window'.\r
- */\r
-\r
- switch(ctty){\r
- default:\r
-\r
- case 0: // Welcome screen\r
-\r
- lcdSetText(uart1,1," JOHN CENA ");\r
- lcdPrint(uart1,2," ===========%3d",motorGet(CANNON1));\r
- break;\r
-\r
- case 1: // Battery levels in volts\r
-\r
- lcdPrint(uart1,1,"MAIN: %0.3f",(float)(powerLevelMain()/1000.0f));\r
- lcdPrint(uart1,2,"EXP : %0.3f",(float)(analogRead(1) / 280.0f));\r
- break;\r
-\r
- case 2: // Gyroscope readings\r
-\r
- imeGet(IROTATER,&ime);\r
+#define PI 3.1415926535L\r
\r
- double rot = ime / 3.0L / 627.2L * 10;\r
-\r
- rot = floor(rot) / 10;\r
-\r
- lcdPrint(uart1,1,"Gyro: %d",gyroGet(gyro));\r
- lcdPrint(uart1,2,"Rot.: %.1lf",rot);\r
-\r
- break;\r
+extern Gyro gyro; // Originally defined in init.c\r
\r
- case 3:\r
+static double Vl, // left wheel velocity\r
+ Vr, // right wheel velocity\r
+ Vc, // center/average velocity\r
+ Va, // angular velocity\r
+ Vra; // velocity ratio\r
\r
- lcdPrint(uart1,1,"%.3lf",imeDriveLeft);\r
- lcdPrint(uart1,2,"%.3lf",imeDriveRight);\r
+static double R; // radius of current circular path\r
\r
- break;\r
- }\r
+static double Px=0, // X position\r
+ Py=0; // Y position\r
\r
- delay(500);\r
- }\r
-}\r
-\r
-void operatorPoll(){\r
- static int idl,idr,ir,gr;\r
+void operatorLCD(void *start){\r
while(1){\r
\r
- /*\r
- * Read in rotations of motors and the current angle of the robot.\r
- */\r
-\r
- imeGet(DRIVEL,&idl); // Get reading\r
- imeDriveLeft = idl / 627.0L; // Convert to wheel rotations\r
- imeDriveLeft *= 8.64L; // Convert to distance in inches\r
-\r
- imeGet(DRIVER,&idr); // Get reading\r
- imeDriveRight = idr / 627.0L; // Convert to wheel rotations\r
- imeDriveRight *= 8.64L; // Convert to distance in inches\r
-\r
- imeGet(ROTATER,&ir);\r
- imeRotater = ir / 627.0L;\r
-\r
- gyroRotation = gyroGet(gyro);\r
+ lcdPrint(uart1,1,"%.3lf, %.3lf",R,Va);\r
\r
delay(100);\r
}\r
}\r
\r
-void operatorControl(){\r
+static unsigned int START;\r
\r
- static bool selfAim = false;\r
+void operatorControl(){\r
\r
static char uiinc = 0; // See below\r
+ static char in=0,lift; // Used to set the multiple cannon motors to the same joy-stick reading.\r
\r
- static char in,lift; // Used to set the multiple cannon motors to the same joy-stick reading.\r
+ static char joy;\r
\r
- taskCreate(operatorLCD ,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT);\r
- taskCreate(operatorPoll,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT);\r
+ /*\r
+ * Start other tasks.\r
+ */\r
+\r
+ START = ((!digitalRead(3)) | (!digitalRead(4)<<1)) + 1;\r
+ taskCreate(operatorLCD ,TASK_DEFAULT_STACK_SIZE,&START,TASK_PRIORITY_DEFAULT);\r
\r
while(1){\r
\r
// Set the drive motors speeds.\r
\r
- motorSet(DRIVEL,joystickGetAnalog(1,3));\r
- motorSet(DRIVER,joystickGetAnalog(1,2));\r
+ joy=joystickGetAnalog(1,3);\r
+ if(joy < 10 && joy > -10)joy=0;\r
+ motorSet(DRIVEL,joy);\r
+ joy=joystickGetAnalog(1,2);\r
+ if(joy < 10 && joy > -10)joy=0;\r
+ motorSet(DRIVER,joy);\r
\r
// Set the rotating motor speed.\r
\r
- if(!selfAim){\r
-\r
- motorSet(ROTATER,-joystickGetAnalog(2,1)/2);\r
-\r
- }else{\r
-\r
- //static int gc=0,gl=0;\r
-\r
- //gl = gc;\r
- //gc = gyroGet(gyro);\r
-\r
- }\r
-\r
- // Set the cannon's speed.\r
+ motorSet(ROTATER,-joystickGetAnalog(2,1)/4);\r
\r
in=joystickGetAnalog(2,3);\r
\r
// Set the intake's speed.\r
\r
motorSet(INTAKE,joystickGetDigital(1,6,JOY_UP )? 127 :\r
- joystickGetDigital(1,6,JOY_DOWN)? -127 :\r
+ joystickGetDigital(1,6,JOY_DOWN)? -127 :\r
0 );\r
\r
// Set the lift's speed.\r
\r
lift=joystickGetDigital(2,6,JOY_UP )? 127 :\r
- joystickGetDigital(2,6,JOY_DOWN)? -127 :\r
- 0 ;\r
+ joystickGetDigital(2,6,JOY_DOWN)? -127 :\r
+ 0 ;\r
\r
motorSet(LIFT1,lift);\r
motorSet(LIFT2,lift);\r
if(++uiinc==20){ // Equates to every 200ms\r
uiinc=0;\r
\r
- // Goto next 'window'.\r
-\r
- if(joystickGetDigital(1,7,JOY_UP) ||\r
- joystickGetDigital(2,7,JOY_UP) ){\r
- if(++ctty==mtty)ctty=0;\r
- }\r
-\r
- // Goto previous 'window'.\r
-\r
- if(joystickGetDigital(1,7,JOY_DOWN) ||\r
- joystickGetDigital(2,7,JOY_DOWN) ){\r
- if(--ctty==-1)ctty=mtty-1;\r
- }\r
-\r
// Run autonomous (for testing wo/ competition switch).\r
\r
if(joystickGetDigital(1,7,JOY_RIGHT))\r
autonomous();\r
\r
- // Goto test area.\r
+ // Test\r
\r
if(joystickGetDigital(1,7,JOY_LEFT)){\r
- static double target = 0;\r
\r
- if(!target) target = (imeDriveLeft+imeDriveRight) / 2 + 13;\r
+ }\r
\r
- motorSet(DRIVEL,60);\r
- motorSet(DRIVER,60);\r
+ }\r
\r
- while((imeDriveLeft+imeDriveRight) / 2 < target)\r
- delay(10);\r
+ delay(10); // Short delay to allow task switching\r
\r
- motorSet(DRIVEL,0);\r
- motorSet(DRIVER,0);\r
+ /*\r
+ * Get the velocity of the two (IME'd) drive motors.\r
+ */\r
\r
- }\r
+ static int vl,vr;\r
\r
- }\r
+ imeGetVelocity(1,&vl);\r
+ imeGetVelocity(0,&vr);\r
\r
- delay(10); // Short delay to allow task switching.\r
- }\r
-}\r
+ /*\r
+ * Convert the raw reading to rotations a minute, then inches per minute, then inches\r
+ * per second.\r
+ */\r
\r
+ Vl = vl /* RPM Divisor */ / 39.2L\r
+ /* Wheel Circumference */ * 8\r
+ /* Minutes to seconds */ / 60;\r
\r
-//Totally Theoretical PseudoCode for AutoShoot\r
-//Based on IME data:\r
-double lVel;//=velocity of left side\r
-double rVel;//=velocity of right side\r
-double cVel;//=velocity of center (calculated below)\r
-double aVel;//=angular velocity (calculated below)\r
+ Vr = -vr / 39.2L * 8 / 60; // Right wheel IME is inverted\r
\r
+ Vc = (Vl + Vr) / 2; // Average/Center velocity\r
\r
-//Used to find rectangular vectors * double aVel=angular velocity of robot\r
-//Used to store the rectangular vectors:\r
-double xVel1;\r
-double xVel2;\r
-double yVel1;\r
-double yVel2;\r
+ /*\r
+ * Round down the results to the nearest inch, and enforce a 2 in/s threshold.\r
+ */\r
\r
+ Vl = floor(Vl);\r
+ Vr = floor(Vr);\r
\r
-//Final Position Vectors, robot and target\r
-double xPos;\r
-double yPos;\r
-double xTarget;\r
-double yTarget;\r
+ if(Vl < 2 && Vl > -2) Vl = 0;\r
+ if(Vr < 2 && Vr > -2) Vr = 0;\r
\r
+ /*\r
+ * Calculate the ratio of the higher velocity to the lowest, for determining the\r
+ * radius of the circle the robot is forming.\r
+ */\r
\r
-//time difference variables\r
-double time1=0;\r
-double time2=0;\r
+ if(((!Vr) & (Vl!=0)) ||\r
+ ((!Vl) & (Vr!=0)) ){\r
\r
-int start;//1 is right blue, 2 is left blue, 3 is right blue, 4 is left blue\r
+ /*\r
+ * One wheel isn't moving, the radius is the distance between the wheels.\r
+ */\r
\r
+ R = 15;\r
+ goto CONT;\r
\r
-//Vector Assignments:\r
-void Vectors(){\r
- if(start==1){xPos=37;yPos=138;xTarget=137;yTarget=7;}\r
- if(start==2){xPos=13;yPos=114;xTarget=137;yTarget=7;}\r
- if(start==3){xPos=37;yPos=13;xTarget=137;yTarget=137;}\r
- if(start==4){xPos=13;yPos=37;xTarget=137;yTarget=137;}\r
- while(1){//something in the brackets\r
+ }else if((Vr > Vl) & (Vl > 0) || // Curve to forwards right\r
+ (Vl > Vr) & (Vl < 0) ){ // Curve to backwards right\r
\r
- imeGetVelocity(DRIVEL,&lVel);//get reading of left velocity\r
- lVel=lVel/24.5*8.64;\r
+ Vra = Vr / Vl;\r
\r
- imeGetVelocity(DRIVER,&rVel);//get reading of right velocity\r
- rVel=rVel/24.5*8.64;\r
+ }else if((Vl > Vr) & (Vr > 0) || // Curve to forwards left\r
+ (Vr > Vl) & (Vr < 0) ){ // Curve to backwards left\r
\r
- int i=0;//counter for use of alternating variables\r
- i++;\r
- if(i==1){//just used the first time\r
- time1=milis();\r
- }\r
- cVel=(lVel+rVel)/2;//calculates the (c)enter of the robot's velocity\r
+ Vra = Vl / Vr;\r
\r
- if(lVel>rVel){\r
- aVel=cVel/(16*lVel/(rVel-lVel));\r
- }\r
+ }else Vra = 0; // No movement?\r
\r
- if(rVel>lVel){\r
- aVel=cVel/(16*rVel/(lVel-rVel));\r
- }\r
+ if(Vra<0) Vra *= -1; // get absolute value of the ratio\r
\r
- else{\r
- aVel=0;\r
+ /*\r
+ * "Plug-n'-chug" for a radius, assuming that the radius will increase by 7.5 (the\r
+ * halfway between the wheels on the robot) when multiplied by the ratio.\r
+ */\r
\r
- if(i%2==0){\r
- xVel1=cos(aVel)*cVel;//every 2 it uses these variables\r
- yVel1=sin(aVel)*cVel;\r
- }\r
+ for(R = 0; R < 200; R++){\r
\r
- else{\r
- xVel2=cos(aVel)*cVel;//otherwise it uses these\r
- yVel2=sin(aVel)*cVel;\r
- }\r
+ if(R * Vra > R + 7 && // Allow a one inch margin of error\r
+ R * Vra < R + 8)break; //\r
+ }\r
\r
- time2=milis();//records the time before calculating\r
+CONT:\r
\r
- xPos+=((xVel1+xVel2)/2)*(time2-time1);//finds the area of the x curve using trapezoidal approx.\r
- yPos+=((yVel1+yVel2)/2)*(time2-time1);//finds the area of the y curve using trapezoidal approx.\r
+ /*\r
+ * Calculate the anglular velocity of the robot.\r
+ */\r
\r
- delay(20);\r
- }\r
- time1=milis();//records time between calculations for next time\r
- }//end of while\r
-}//end of Vectors\r
+ Va = Vc / R;\r
\r
+ /*\r
+ * Determine the increase in X and Y position based on the angular velocity and the\r
+ * average total movement.\r
+ */\r
+\r
+ Px += cos(Va) * Vc * .01; // Convert per second velocity to per 10ms, as the motors\r
+ Py += sin(Va) * Vc * .01; // have only been at this velocity for that time (above).\r
+\r
+ }\r
+}\r
projects / abelleisle / vex5106z.git / commitdiff