 | Code: #pragma config(Sensor, in1, lineFollowerRIGHT, sensorLineFollower)
#pragma config(Sensor, in2, lineFollowerCENTER, sensorLineFollower)
#pragma config(Sensor, in3, lineFollowerLEFT, sensorLineFollower)
#pragma config(Sensor, dgtl1, armEncoder, sensorQuadEncoder)
#pragma config(Sensor, dgtl3, rightEncoder, sensorQuadEncoder)
#pragma config(Sensor, dgtl5, leftEncoder, sensorQuadEncoder)
#pragma config(Sensor, dgtl7, clawSwitch, sensorTouch)
#pragma config(Sensor, dgtl8, sonarSensor, sensorSONAR_inch)
#pragma config(Motor, port1, leftMotor, tmotorNormal, openLoop)
#pragma config(Motor, port2, arm1, tmotorNormal, openLoop, reversed)
#pragma config(Motor, port3, arm2, tmotorNormal, openLoop, reversed)
#pragma config(Motor, port4, ext1Motor, tmotorNormal, openLoop, reversed)
#pragma config(Motor, port5, ext2Motor, tmotorNormal, openLoop, reversed)
#pragma config(Motor, port6, , tmotorNormal, openLoop)
#pragma config(Motor, port7, arm3, tmotorNormal, openLoop)
#pragma config(Motor, port8, arm4, tmotorNormal, openLoop)
#pragma config(Motor, port9, clawMotor, tmotorNormal, openLoop)
#pragma config(Motor, port10, rightMotor, tmotorNormal, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
#pragma platform(VEX)
//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)
// Declare Functions
void Claw(float c);
void Sonar(float s);
void ArmPos(float ap);
void Forward(float f);
void TurnLeft(float l);
void TurnRight(float r);
void Backward(float b);
// Declare Global Variables /* 'rotations' will be a counter for every 360 encoder clicks */
const float rotations = 360.0; /* which is one full rotation of the wheel (ie. 2 'rotations' */
/* will equal 720.0 clicks, 2 full rotations of the wheel). */
int SonarValue;
int ArmLoc = SensorValue(armEncoder);
#include "Vex_Competition_Includes.c" //Main competition background code...do not modify!
////////////////////////////////////////////////////////////////////////////////////////////////
// //
// Pre-Autonomous Functions //
// //
// You may want to perform some actions before the competition starts. Do them in the //
// following function. //
// //
////////////////////////////////////////////////////////////////////////////////////////////////
void pre_auton()
{
// All activities that occur before the competition starts
// Example: clearing encoders, setting servo positions, ...
SensorValue(rightEncoder) = 0; /* Clear the encoders for */
SensorValue(leftEncoder) = 0; /* consistancy and accuracy. */
SensorValue(armEncoder) = 0;
}
////////////////////////////////////////////////////////////////////////////////////////////////
// //
// Autonomous Task //
// //
// This task is used to control your robot during the autonomous phase of a VEX Competition. //
// You must modify the code to add your own robot specific commands here. //
// //
////////////////////////////////////////////////////////////////////////////////////////////////
//+++++++++++++++++++++++++++++++++++++++++++++| Autonomous |+++++++++++++++++++++++++++++++++++++++++++++++
task autonomous()
{
wait1Msec(2000); // Wait 2000 milliseconds before continuing.
int i;
for(i=0; i<1; i++) // While 'i' is less than 1:
{
//Sonar(10.0); // Call function 'Sonar(float)' and pass the float value '10.0' through.
ArmPos(2.0); // Call function 'ArmPos(float)' and pass the float value '1.0' through.
Forward(1.0); // Call function 'Forward(float)' and pass the float value '1.0' through.
Claw(1.0); // Call function 'Claw(float)' and pass the bool value '1.0' through.
//ArmPos(40.0); // Call function 'ArmPos(float)' and pass the float value '40.0' through.
Backward(2.0); // Call function 'Backward(float)' and pass the float value '1.0' through.
Claw(-1.0); // Call function 'Claw(float)' and pass the bool value '-1.0' through.
TurnLeft(1.19); // Call function 'TurnLeft(float)' and pass the float value '1.19' through.
Forward(2.0); // Call function 'Forward(float)' and pass the float value '2.0' through.
TurnRight(1.19); // Call function 'TurnRight(float)' and pass the float value '1.19' through.
}
}
//----------------------------------------| FORWARD FUNCTION |-----------------------------------------
void Forward(float f)
{
SensorValue(rightEncoder) = 0; /* Clear the encoders for */
SensorValue(leftEncoder) = 0; /* consistancy and accuracy. */
// While the encoders have not yet met their goal: (r * rotations) ie (3.0 * 360.0) or "three rotations"
while(SensorValue(rightEncoder) < (f * rotations) && SensorValue(leftEncoder) < (f * rotations))
{
motor[rightMotor] = 80; /* Run both motors */
motor[leftMotor] = 80; /* forward at half speed. */
}
motor[rightMotor] = 0; /* Stop both motors! This is important so that each function */
motor[leftMotor] = 0; /* can act independantly as a "chunk" of code, without any loose ends. */
}
//----------------------------------------------------------------------------------------------------
//--------------------------------------| CLAW FUNCTION (true) |------------------------------------------
void Claw(float c)
{
if (c == 1);
{
motor[clawMotor] = 127; /* Run the clawMotor */
} /* forward at full speed. */
if (c == -1);
{
while (SensorValue(clawSwitch) == 0);
{
motor[clawMotor] = -127; /* Run the clawMotor */
} /* backward at full speed. */
}
}
//----------------------------------------------------------------------------------------------------
//----------------------------------------| SONAR FUNCTION |-------------------------------------------
void Sonar(float s)
{
SonarValue = SensorValue(sonarSensor);
while(SensorValue(sonarSensor) > s)//Loop while robot's Ultrasonic sensor is further than S inches away from an object
{
motor[rightMotor] = 70; // Motor on port2 is run at 70 power forward
motor[leftMotor] = 70; // Motor on port3 is run at 70 power forward
}
}
//----------------------------------------------------------------------------------------------------
//----------------------------------------| ARMPOSITION FUNCTION |-----------------------------------------
void ArmPos(float ap)
{
ArmLoc = SensorValue(armEncoder);
// While the encoder has not yet met its goal
while(ArmLoc < ap)
{
motor[arm1] = 70;
motor[arm2] = 70;
motor[arm3] = 70;
motor[arm4] = 70;
ArmLoc = SensorValue(armEncoder);
}
// While the encoder has not yet met its goal
while(ArmLoc > ap)
{
motor[arm1] = -70;
motor[arm2] = -70;
motor[arm3] = -70;
motor[arm4] = -70;
ArmLoc = SensorValue(armEncoder);
}
motor[arm1] = 0; /* Stop the motor! This is important so that each function */
motor[arm2] = 0; /* can act independantly as a "chunk" of code, without any loose ends. */
motor[arm3] = 0;
motor[arm4] = 0;
}
//----------------------------------------------------------------------------------------------------
//----------------------------------------| BACKWARD FUNCTION |----------------------------------------
void Backward(float b)
{
SensorValue(rightEncoder) = 0; /* Clear the encoders for */
SensorValue(leftEncoder) = 0; /* consistancy and accuracy. */
// While the encoders have not yet met their goal: (r * rotations) ie (3.0 * 360.0) or "three rotations"
while(SensorValue(rightEncoder) > -(b * rotations) && SensorValue(leftEncoder) > -(b * rotations))
{
motor[rightMotor] = -80; /* Run both motors */
motor[leftMotor] = -80; /* forward at half speed. */
}
motor[rightMotor] = 0; /* Stop both motors! This is important so that each function */
motor[leftMotor] = 0; /* can act independantly as a "chunk" of code, without any loose ends. */
}
//----------------------------------------------------------------------------------------------------
//---------------------------------------| TURN LEFT FUNCTION |----------------------------------------
void TurnLeft(float l)
{
SensorValue(rightEncoder) = 0; /* Clear the encoders for */
SensorValue(leftEncoder) = 0; /* consistancy and accuracy. */
// While the encoders have not yet met their goal: (left is compared negativly since it will in reverse)
while(SensorValue(rightEncoder) < (l * rotations) && SensorValue(leftEncoder) > (-1 * l * rotations))
{
motor[rightMotor] = 80; // Run the right motor forward at 100 speed
motor[leftMotor] = -80; // Run the left motor backward at 100 speed
}
motor[rightMotor] = 0; /* Stop both motors! This is important so that each function */
motor[leftMotor] = 0; /* can act independantly as a "chunk" of code, without any loose ends. */
}
//----------------------------------------------------------------------------------------------------
//---------------------------------------| TURN RIGHT FUNCTION |---------------------------------------
void TurnRight(float r)
{
SensorValue(rightEncoder) = 0; /* Clear the encoders for */
SensorValue(leftEncoder) = 0; /* consistancy and accuracy. */
// While the encoders have not yet met their goal: (left is compared negativly since it will in reverse)
while(SensorValue(leftEncoder) < (r * rotations) && SensorValue(rightEncoder) > (-1 * r * rotations))
{
motor[rightMotor] = -80; // Run the right motor forward at half speed
motor[leftMotor] = 80; // Run the left motor backward at half speed
}
motor[rightMotor] = 0; /* Stop both motors! This is important so that each function */
motor[leftMotor] = 0; /* can act independantly as a "chunk" of code, without any loose ends. */
}
//----------------------------------------------------------------------------------------------------
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
////////////////////////////////////////////////////////////////////////////////////////////////
// //
// User Control Task //
// //
// This task is used to control your robot during the user control phase of a VEX Competition.//
// You must modify the code to add your own robot specific commands here. //
// //
////////////////////////////////////////////////////////////////////////////////////////////////
//=============================================| DRIVE |==============================================\\
task Drive()
{
int joy_l; // Will hold the X value of the LEFT analog stick.
int joy_r; // Will hold the X value of the RIGHT analog stick.
int threshold = 20; // Helps to eliminate 'noise' from a joystick that isn't perfectly at (0,0)
while(true)
{
joy_l = vexRT[Ch3]; //This gets the X axis value of the LEFT analog stick and assigns the value to "joy_l".
joy_r = vexRT[Ch2]; //This gets the X axis value of the RIGHT analog stick and assigns the value to "joy_r".
////////////////////////////////////////////////////////////////////////////////////////////////////////////
//If joy_l is above the threshhold then assign that vale to leftMotor, if not leftMotor power equals 0. //
//
if(abs(joy_l) > threshold) //
{ //
motor[leftMotor] = (joy_l); //
} //
//
else //
{ //
motor[leftMotor] = 0; //
} //
//
////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////
//If joy_r is above the threshhold then assign that vale to rightMotor, if not rightMotor power equals 0. //
//
if(abs(joy_r) > threshold) //
{ //
motor[rightMotor] = (joy_r); //
} //
//
else //
{ //
motor[rightMotor] = 0; //
} //
//
////////////////////////////////////////////////////////////////////////////////////////////////////////////
wait1Msec(25); // helps the multitasking share evenly
}
}
//====================================================================================================\\
//==============================================| Arm |===============================================\\
task Arm()
{
{
int armPosition = SensorValue(armEncoder);
while(true)
{
/////////////////////////////Claw Movement code////////////////////////////////////////////////
/* if (SensorValue(clawSwitch) == 1);
{
motor(clawMotor) = 127;
}
if (SensorValue(clawSwitch) == 0);
{
motor(clawMotor) = 0;
}*/
if (vexRT[Btn8LXmtr2] == 1)
{
motor(clawMotor) = 127;
}
else
{
if (vexRT[Btn8RXmtr2] == 1)
{
//while(SensorValue(clawSwitch) == 1);
//{
motor(clawMotor) = -100;
//}
}
}
if ((vexRT[Btn8LXmtr2] == 0) && (vexRT[Btn8RXmtr2] == 0));
{
motor(clawMotor) = 0;
}
///////////////////////////End Claw Movement code////////////////////////////////////////////////
//////////////////////////////Arm Movement code//////////////////////////////////////////////////
if (vexRT[Btn8UXmtr2] == 1)
{
motor[arm1] = 100;
motor[arm2] = 100;
motor[arm3] = 100;
motor[arm4] = 100;
}
if (vexRT[Btn8DXmtr2] == 1)
{
motor[arm1] = -100;
motor[arm2] = -100;
motor[arm3] = -100;
motor[arm4] = -100;
}
if ((vexRT[Btn8UXmtr2] == 0) && (vexRT[Btn8DXmtr2] == 0))
{
motor[arm1] = 0;
motor[arm2] = 0;
motor[arm3] = 0;
motor[arm4] = 0;
}
///////////////////////////////End Arm Movement code////////////////////////////////////////////////
/////////////////////////////Hang Arm Extention code////////////////////////////////////////////////
if (vexRT[Btn5DXmtr2] == 1)
{
motor(ext2Motor) = 127;
}
else
{
motor(ext2Motor) = 0;
}
if (vexRT[Btn5UXmtr2] == 1)
{
motor(ext2Motor) = -127;
}
if (vexRT[Btn6DXmtr2] == 1)
{
motor(ext1Motor) = 127;
}
else
{
motor(ext1Motor) = 0;
}
if (vexRT[Btn6UXmtr2] == 1)
{
motor(ext1Motor) = -127;
}
///////////////////////////End Hang Arm Extention code////////////////////////////////////////////////
}
}
}
//====================================================================================================\\
task usercontrol()
{
//+++++++++++++++++++++++++++++++++++++++++++++| MAIN |+++++++++++++++++++++++++++++++++++++++++++++++\\
StartTask(Drive);
StartTask(Arm);
int batteryLevel; //Variable to hold the battery level.
while(true) // infinite loop, to keep the program alive
{
batteryLevel = (nAvgBatteryLevel); //"batteryLevel" equals average battery level in Mili-Volts
wait1Msec(25); // helps the multitasking share evenly
}
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\\
} | |
