void lineTrackForRotations(float rotations = 3.0, int threshold = 1500, tSensors leftSensorPort = in1, tSensors centerSensorPort = in2, tSensors rightSensorPort = in3)
{
SensorValue[dgtl1] = 0;
SensorValue[dgtl3] = 0;
while((SensorValue[dgtl1] < (rotations * 360)) && (SensorValue[dgtl3] < (rotations * 360)))
{
// RIGHT sensor sees dark:
if(SensorValue(rightSensorPort) > threshold)
{
// counter-steer right:
motor[port2] = 0;
motor[port3] = 45;
}
// CENTER sensor sees dark:
else if(SensorValue(centerSensorPort) > threshold)
{
// go straight
motor[port2] = 45;
motor[port3] = 45;
}
// LEFT sensor sees dark:
else if(SensorValue(leftSensorPort) > threshold)
{
// counter-steer left:
motor[port2] = 45;
motor[port3] = 0;
}
wait1Msec(1);
}
}
task LCD() {
bLCDBacklight = true; // Turn on LCD Backlight
string mainBattery, backupBattery;
while(true) // An infinite loop to keep the program running until you terminate it
{
clearLCDLine(0); // Clear line 1 (0) of the LCD
clearLCDLine(1); // Clear line 2 (1) of the LCD
//Display the Primary Robot battery voltage
displayLCDString(0, 0, "Primary: ");
sprintf(mainBattery, "%1.2f%c", nImmediateBatteryLevel/1000.0,'V'); //Build the value to be displayed
displayNextLCDString(mainBattery);
//Display the Backup battery voltage
//displayLCDString(1, 0, "Backup: ");
//sprintf(backupBattery, "%1.2f%c", BackupBatteryLevel/1000.0, 'V'); //Build the value to be displayed
//displayNextLCDString(backupBattery);
//Sensor values
displayLCDNumber(1, 0, -SensorValue(TensionEncoder));
displayLCDNumber(1, 4, SensorValue(BowEncoder));
//Short delay for the LCD refresh rate
wait1Msec(100);
}
}
void pneumatics()
{
if(vexRT[Btn6U]==1)
SensorValue(Solenoid)=1;
else if(vexRT[Btn6D]==1)
SensorValue(Solenoid)=0;
if(vexRT[Btn8U]==1)
{
motor[wheelMotor]=120;
motor[wheelMotor1]=120;
motor[wheelMotor2]=120;
}
else if(vexRT[Btn8L]==1)
{
motor[wheelMotor]=60;
motor[wheelMotor1]=60;
motor[wheelMotor2]=60;
}
else if(vexRT[Btn8D]==1)
{
motor[wheelMotor]=0;
motor[wheelMotor1]=0;
motor[wheelMotor2]=0;
}
}
//----Turns if need to----//
void doTurn()
{
//part of the testing reigme of the local cells
//decided that anticlockwise is positive
float rightSonarValue = SensorValue(rightSonar); //obvious
float leftSonarValue = SensorValue(leftSonar);
float centreSonarValue = SensorValue(centreSonar);
if(centreSonarValue<19)
{
if(leftSonarValue > 19 && rightSonarValue < 19)
{
drive(100,190,-50);
drive(-100,190,-50);
}
else if(leftSonarValue < 19 && rightSonarValue > 19)
{
drive(100,190,-50);
drive(-100,190,50);
}
else if(leftSonarValue < 19 && rightSonarValue < 19)
{
drive(100,190,-50);
drive(-100,370,50);
}
else
{
drive(100,190,-50);
drive(-100,190,-50);
}
}
}
void alignToLight() {
/* At the beginning of the maze, we need to find the light and orient our robot towards the light
** We will rotate until the strongest light source (which should be the lantern) is detected.
*/
float leftLightValue=SensorValue(leftLightSensor);
float maximumLight = leftLightValue;
int counter = 0;
for (int i=0; i<90; i++) {
// Initial loop to locate the maximum value. We do not want to locate the maximum value and then continue to turn. So we
// loop as long as maximumLight is NOT less than the sensor value.
writeDebugStreamLine("LIGHT VALUE: %d MaximumLight: %d", SensorValue(leftLightSensor),maximumLight);
if (maximumLight > SensorValue(leftLightSensor)) {
maximumLight=SensorValue(leftLightSensor);
writeDebugStreamLine("Max Value: %d", maximumLight);
counter =0;
}
counter++;
writeDebugStreamLine("Counter: %d",counter);
turnLeft(100);
}
for (int i = counter; i>0; i--) {
//Should rotate back to the right exactly as much as the robot had turned to the left.
turnRight(100);
writeDebugStreamLine("CountDOWN: %d", i);
}
}
void lineTrackForTime(float trackTime = 5.0, int threshold = 1500, tSensors leftSensorPort = in1, tSensors centerSensorPort = in2, tSensors rightSensorPort = in3)
{
float timeStart = ((float)nPgmTime / 1000);
while(((float)nPgmTime / 1000) - timeStart < trackTime)
{
// RIGHT sensor sees dark:
if(SensorValue(rightSensorPort) > threshold)
{
// counter-steer right:
motor[port2] = 0;
motor[port3] = 45;
}
// CENTER sensor sees dark:
else if(SensorValue(centerSensorPort) > threshold)
{
// go straight
motor[port2] = 45;
motor[port3] = 45;
}
// LEFT sensor sees dark:
else if(SensorValue(leftSensorPort) > threshold)
{
// counter-steer left:
motor[port2] = 45;
motor[port3] = 0;
}
wait1Msec(1);
}
}
task main()
{
//Loop Forever
while(1 == 1)
{
int distance_int = SensorValue(distance);
if (SensorValue[sideButton] == 1){
goToBrightestLight();
} else if(vexRT[Btn7D] == 0 && vexRT[Btn8D] == 0){
// manual driving
if(distance_int >= 0 && distance_int <= 6 && vexRT[Ch3] > 0 && vexRT[Ch2] > 0){
// too close to a wall, stop
motor(leftMotor) = 0;
motor(rightMotor) = 0;
SensorValue(red) = 1;
SensorValue(green) = 0;
} else {
motor(leftMotor) = vexRT[Ch3];
motor(rightMotor) = vexRT[Ch2];
SensorValue(red) = 0;
SensorValue(green) = 1;
}
} else if(vexRT[Btn7D] == 1 && vexRT[Btn8D] == 0){
goForwardInches(6, true);
} else if(vexRT[Btn8D] == 1){
goToBrightestLight();
wait1Msec(100);
}
}
}
void initializeRobot()
{
const byte max = 127;
//Lift raises at max speed until top button is pressed.
motor[Lift] = max;
while (!SensorValue(LiftTouchHigh)) {}
motor[Lift] = 0;
nMotorEncoder[Lift] = 0; // THIS VALUE MUST BE THE VALUE OF THE POSITION OF THE TOUCH SENSOR
while (SensorValue(LiftTouchHigh))
bump(Lift, false);
//Reset Lift encoder. UNCOMMENT IF LIFT ENCODER IS WIRED.
nMotorEncoder[Lift] = 0;
//Move Lift down to center.
bump(Lift, false);
//Arm retracts at max speed until button is pressed.
motor[Arm] = -max;
while (!SensorValue(ArmTouch)) {}
motor[Arm] = 0;
nMotorEncoder[Arm] = 0; // THIS VALUE MUST BE THE VALUE OF THE POSITION OF THE TOUCH SENSOR
while (SensorValue(ArmTouch))
bump(Arm, true);
//Reset all drive encoders. UNCOMMENT IF DRIVER ENCODERS ARE WIRED.
//resetDriveEncoders();
//Set Scoop to full down position.
servo[Scoop] = 255;
}
int findBrightestLight(){
int minLightSensor = 500;
int potValue = 0;
while(SensorValue(pot) > 500){
motor(sensorMotor) = 15;
//writeDebugStreamLine("Counter Clockwise %d - %d",SensorValue(pot),SensorValue(lightSensor));
wait1Msec(100);
}
stopMotor(sensorMotor);
while(SensorValue(pot) < 3500){
motor(sensorMotor) = -13;
//writeDebugStreamLine("Clockwise %d - %d",SensorValue(pot),SensorValue(lightSensor));
if(SensorValue(lightSensor) < minLightSensor){
minLightSensor = SensorValue(lightSensor);
potValue = SensorValue(pot);
}
wait1Msec(100);
}
//writeDebugStreamLine("Stopped");
//writeDebugStreamLine("Lowest value %d at potentiometer %d", minLightSensor, potValue);
stopMotor(sensorMotor);
potValue = ((potValue-573.0) * (180.0/2698.0)) - 10;
return potValue;
}
//+++++++++++++++++++++++++++++++++++++++++++++| MAIN |+++++++++++++++++++++++++++++++++++++++++++++++
task main()
{
// initialize variables for current and target angles
int current_angle = 0;
int target_angle = 0;
// initialize variable to use in FOR loop
int i = 0;
// loop through 9 times for a total of 10 measurements (0 through 9)
for (i = 0; i < 9; i++)
{
// read the current potentiometer value and save it to a variable
current_angle = SensorValue(angleSensor);
// wait 5 seconds so you can read the sonar sensor value off the screen
wait1Msec(5000);
// set the target angle to the current angle + some number (you will have to determine this by trial and error)
target_angle = current_angle + 10;
// run the motor until the potentiometer reads the same value as the target angle
while (SensorValue(angleSensor) < target_angle)
{
motor[scanMotor] = 25;
}
motor[scanMotor] = 0;
}
}
void setArm() // reads Joystick, determines arm motor values and sends values to motors - used in driver control
{ // uses PID to hold the arm up when buttons are released
// note: this function does not decide where the upper/lower limits are, setArmMotors does
int currentPosition = SensorValue(leftArmPot);
if (vexRT[Btn5U] == 1) // if button 5 Up is pressed...
{
bHoldHeight = false; // ...don't hold position, instead...
armPid.goal = LEFT_ARM_UPPER_LIMIT; // ...set the goal to the upper limit.
}
else if (vexRT[Btn5D] == 1) // if button 5 Down is pressed...
{
bHoldHeight = false; // ...don't hold position, instead...
armPid.goal = LEFT_ARM_LOWER_LIMIT; // ...set the goal to the lower limit.
}
else // if neither button is pressed...
{
if (bHoldHeight == false) // ...and we weren't previously holding position...
{
bHoldHeight = true; // ...now we want to hold position, so...
armPid.goal = SensorValue(leftArmPot); // ...set the goal to the current height.
}
}
if (time1(T3) > PID_UPDATE_TIME) // if enough time has passed since the last PID update...
{
float output = UpdatePid(armPid, currentPosition); // ...update the motor speed with the arm PID...
setArmMotors(output); // ...and send that speed to the arm motors...
ClearTimer(T3); // ...and reset the timer for the next update.
}
}
task autonomous()
{
SensorType[in8] = sensorNone;
wait1Msec(1000);
//Reconfigure Analog Port 8 as a Gyro sensor and allow time for ROBOTC to calibrate it
SensorType[in8] = sensorGyro;
wait1Msec(2000);
if(SensorValue(limit1) == 0) //hang a cube from the back, move backwards until the limit switch is pressed
{
driveStraight1(-100);
}
else if(SensorValue(limit1) == 1)
{
wait1Msec(1000); //wait for the cube to release from the robot
driveStraight1(100);
}
// gyroTurn(45);//turns 45 degrees
turn(100);
wait1Msec(1000);
turn(0);
driveStraight(sqrt(2), 50);//returns to starting position
driveStraight(1.5, 50);
//do something with intake
if(SensorValue(limit1) == 0)
{
driveStraight1(-100);
}
}
task main()
{
int speed = 0; // Will hold the speed of the motors.
int sonarValue = 0; // Will hold the values read in by the Sonar Sensor.
int distance = 100; // Specified distance to be at 35 centimeters.
while(true) // (infinite loop, also represented by 'while(1)' or, if you are feeling devious, 'for(;;)' which is read as 'for ever').
{
sonarValue = SensorValue(sonarSensor); // Store Sonar Sensor values in 'sonarValue' variable.
nxtDisplayCenteredTextLine(0, "Sonar Reading"); /* Display Sonar Sensor values */
nxtDisplayCenteredBigTextLine(2, "%d", sonarValue); /* to LCD screen using %d. */
wait1Msec(100); // Only update the screen every 100 milliseconds.
speed = (SensorValue(sonarSensor) - distance); // Variable 'speed' is set to the reading of the Sonar Sensor - some distance in centimeters (here we used 35cm).
if(speed > 50)
{
speed = 30; // Check to see if calculated speed is greater than 100, if so make it 100.
}
nxtDisplayCenteredTextLine(5, "%d", speed); /* Display variable 'speed' to the LCD. */
nxtDisplayCenteredTextLine(7, "Motor Speed"); /* (which is the current speed of the motors) */
motor[motorC] = speed; // Set Motor C is run at a power level equal to 'speed'.
motor[motorB] = speed; // Set motor B is run at a power level equal to 'speed'.
}
}
task main()
{
//waitForStart();// Waiting for start command from FCS
while(true)// Command recieved start loop untill FCS closes program
{
g = SensorValue(Gyro);
i = SensorValue(IR);
getJoystickSettings(joystick); //Refresh joystick values
if (joy1Btn(6) || joy1Btn(8)) {drive_power = drive_max_power;} //If joystick 1 button TR or BR are pushed set drive power to boost mode, otherwise set it to normal power
else {drive_power = drive_min_power;} //If joystick 1 button TR or BR are pushed set drive power to boost mode, otherwise set it to normal power
initial_power(); //Set the global power values
if (joy1Btn(7) || joy1Btn(5)) {inverse_slave();} //If joystick 1 button RL or BL are pushed run the subrutine to rotate in place
if (joy1Btn(3) || joy2Btn(3)) {reset();} //If joystick 1 or joystick 2 push the red b button set all motor speeds to 0 unitll release
if (joy1Btn(1) || joy1Btn(2)) {slave();} //If joystick 1 button A or X are pressed, slave the two drive motors together
if (joy2Btn(5)) {paddeler(1);} //If joystick 2 button TL is pushed, move the paddles at speed tower_paddle_power
if (joy2Btn(7)) {paddeler(-1);} //If joystick 2 button TL is pushed, move the paddles at negative speed tower_paddle_power
if (joy2Btn(2)) {tower_flag = tower_flag_power;} //If joystick 2 button A is pressed, run flag motor forward
if (joy2Btn(4)) {tower_flag = -1* tower_flag_power;} //If joystick 2 button Y is pressed, run flag motor backwards
send_debug();//Send Debugging info to debugger nxt
enact_power();//Enact the power variables layed out by the previous subrutines
}
}
void followLeftWall() {
/* Invoked when robot is driven and limit switches are activated.
** If the limit switches are activated then the robot is traveling
** in a direction that is somewhat close to being parrallel of the wall.
** We should correct our direction (to avoid a crash) and oscillate on the wall.
*/
writeDebugStreamLine("In followLeftWall");
while(SensorValue(frontRightBumper) == 0) {
driveMotors(-127,127);
wait1Msec(1000);
writeDebugStreamLine("In the first while loop of followLeftWall");
while ((SensorValue(leftLimitSensor) == 0) && (SensorValue(frontRightBumper) == 0)) {
writeDebugStreamLine("LEFT LIMIT SENSOR IS 0");
driveMotors(-127,127);
wait1Msec(200);
}
while ((SensorValue(leftLimitSensor) == 1) && (SensorValue(frontRightBumper) == 0) ) {
writeDebugStreamLine("LEFT LIMIT SENSOR IS 1");
stopMotors(300);
turnRight(1200);
driveMotors(-127,127);
wait1Msec(2000);
stopMotors(300);
turnLeft(1000);
}
driveMotors(-70,70);
wait1Msec(100);
}
}
task main()
{
// run forever:
while(true)
{
// go forward at speed 75:
motor[rightMotor] = 75;
motor[leftMotor] = 75;
// if an object is detected closer than our 'threshold':
if(SensorValue(sonarSensor) < threshold && SensorValue(sonarSensor) != -1)
{
// HALT and back up!:
motor[rightMotor] = -75;
motor[leftMotor] = -75;
wait1Msec(1000);
// turn until the path ahead seems clear:
while(SensorValue(sonarSensor) < (threshold * 2))
{
// turn right in place at speed 75:
motor[rightMotor] = -75;
motor[leftMotor] = 75;
}
}
}
}
void ServoMove(int servoPosition){
int light_value =SensorValue (Light_sensor);
int sonar_value = SensorValue(Sonar);
writeDebugStreamLine("Servo Position = %d, Light Sensor = %d, Sonar = %d",servoPosition, light_value, sonar_value);
motor[Servo] = servoPosition;
wait1Msec(50);
}
/*+++++++++++++++++++++++++++++++++++Task Main+++++++++++++++++++++++++++++++++++*/
task main(){
while(1){
//run the fly wheel control procedures
FwControlTask(left_FW, SensorValue(leftFwEncoder));
FwControlTask(right_FW, SensorValue(rightFwEncoder));
}
}
void pre_auton()
{
// Set bStopTasksBetweenModes to false if you want to keep user created tasks running between
// Autonomous and Tele-Op modes. You will need to manage all user created tasks if set to false.
bStopTasksBetweenModes = true;
SensorValue(left) = 0;
SensorValue(right) =0;
}
void driveStraight(float dist, int speed)
{
while((SensorValue(dgtl1) < 360 * dist / (4 * PI) + error) || (SensorValue(dgtl1) < 360 * dist / (4 * PI) + error))
motor[DFR] = speed;
motor[DBR] = speed;
motor[DFL] = speed;
motor[DBL] = speed;
}
void driveWithin(int distanceInCm){
int sonarVal = SensorValue(sonar);
while(sonarVal > distanceInCm){
motor[leftMotor] = 50;
motor[rightMotor] = 50;
sonarVal = SensorValue(sonar);
//writeDebugStreamLine("the sonar value is %d", sonarVal);
}
}
task pneumatics(){
if(strafe){
SensorValue(leftPneumatics) = 1;
SensorValue(rightPneumatics) = 1;
}
else{
SensorValue(leftPneumatics) = 0;
SensorValue(rightPneumatics) = 0;
}
}
void testLimitSwitch() {
wait1Msec(2000);
// Loop while robot's sensor is not pressed
while (SensorValue(leftLimitSensor) == 0 ) {
motor[leftMotor] = 23;
motor[rightMotor] = 23;
writeDebugStreamLine("Right Limit Sensor: %d", SensorValue(rightLimitSensor));
}
stopMotors(2000);
}
////// PID TURN //////
void TurnLeft (int Angle) {
long error = 99999;
float KP = 0.35;
float KD = 1.5;
float KI = 1;
long Velocity = 0;
long Integral = 0;
long PreviousAngle = 0;
SensorValue(TurnGyro) = 0;
int MotorSpeed = 0;
int cycle = 0;
while (abs(error) > 10) {
// P = calculate the error
error = Angle + SensorValue(TurnGyro);
// D = Calculate the speed
Velocity = -SensorValue(TurnGyro) - PreviousAngle;
if (Velocity < 10){
if(error > 0){
Integral++;
}
else {
Integral = 0;
}
}
else {
Integral = 0;
}
// I = how long we have been away
if (abs(error) < 10){
// If we are close add to cycle
cycle = cycle + 1;
}
else if (abs(Velocity) < 10){
// If we are going slow
cycle = cycle + 1;
}
// Calculates how fast to move the robot
MotorSpeed = (error * KP) + (Integral * KI) - (Velocity * KD);
BaseSpeed(-MotorSpeed, MotorSpeed);
writeDebugStreamLine("Cycle: %d", cycle);
// Save the distance for the next cycle
PreviousAngle = -SensorValue(TurnGyro);
wait1Msec(25);
}
BaseSpeed(0,0);
}
task main()
{
startTask(geluid);
while(1)
{
startTask(accelerate);
while (SensorValue(sonarsensor) > 5)
{
startTask(geluid);
while (SensorValue(sonarsensor) > 30)
{
if ((SensorValue(EyeLeft) == BLACKCOLOR) && (SensorValue(EyeRight) <= rightThreshold))
{
//
// Beide sensoren zien lijn, oftewel kruispunt.
//
clearSounds();
motor[left] = 0;
motor[right] = 0;
}
if ((SensorValue(EyeLeft) == BLACKCOLOR)&&(SensorValue(EyeRight)> rightThreshold))
{
//
// Only left sensor sees the line, we must turn left.
//
motor[left] = i;
motor[right] = i/(0.1 * speedlimit);
}
if ((SensorValue(EyeLeft) == WHITECOLOR) &&(SensorValue(EyeRight)< rightThreshold))
{
//
// Only right sensor sees the line, turn right.
//
motor[left] = i/(0.1 * speedlimit);
motor[right] = i;
}
if ((SensorValue(EyeLeft) == WHITECOLOR)&&(SensorValue(EyeRight) > rightThreshold))
{
motor[left] = i/2;
motor[right] = i/2;
}
wait1Msec(5);
}
clearSounds();
startTask(deaccelerate);
}
clearSounds();
i = 0;
}
}
//+++++++++++++++++++++++++++++++++++++++++++++| MAIN |+++++++++++++++++++++++++++++++++++++++++++++++
task main()
{
wait1Msec(2000); // Robot waits for 2000 milliseconds before executing program
while(SensorValue(sonarSensor) > 20 || SensorValue(sonarSensor) == -1) // Loop while robot's Ultrasonic sensor is further than 20 inches away from an object
{ // || (or) it is '-1'. (-1 is the value returned when nothing is in it's visable range)
motor[rightMotor] = 63; // Motor on port2 is run at half (63) power forward
motor[leftMotor] = 63; // Motor on port3 is run at half (63) power forward
}
}
void setArmMotors(int value) // lets you set the arm motor values in one call
{ // this function also limits the motors at the top/bottom of arm travel for safety
if (value > 0 && SensorValue(leftArmPot) > LEFT_ARM_UPPER_LIMIT) // if we're above the upper limit and going up...
{ value = 0; } // ...set the arm motor value to 0
else if (value < 0 && SensorValue(leftArmPot) < LEFT_ARM_LOWER_LIMIT) // if we're below the lower limit and going down...
{ value = 0; } // ...set the arm motor value to 0
motor[rightTopArm] = value; // rightTop - 4
motor[rightBottomArm] = value; // rightBottom - 5
motor[leftTopArm] = value; // leftTop - 6
motor[leftBottomArm] = value; // leftBottom - 7
}
void move_to_bottom(){
while(SensorValue(bottom_lever) == 0)
move_motors(-15);
if(SensorValue(bottom_lever) == 1)
move_motors(0);
SensorValue[shaft_1] = 0;
return;
}
void scan(){
nMotorEncoder[motorB] = 0;
nMotorEncoder[motorC] = 0;
motor[motorC] = 20;
motor[motorB] = -20;
for(int i=angle;i<=max;i+=angle){
while ((nMotorEncoder[motorC] < i) || (nMotorEncoder[motorB] > -i ) ){
}
//while the encoder wheel turns one revolution
int x=(i/angle)-1;
left1[x]=SensorValue(sleft);
right1[x]=SensorValue(sright);
/*
nxtEraseRect(0, 62, 62, 0);
int f=nMotorEncoder[motorC];
nxtDisplayStringAt(10, 40, "%d %d", f, nMotorEncoder[motorB]);
*/
}
motor[motorB] = 0;
motor[motorC] = 0;
nMotorEncoder[motorB] = 0;
nMotorEncoder[motorC] = 0;
motor[motorC] = -20;
motor[motorB] = 20;
for(int i=angle;i<=max;i+=angle){
while ((nMotorEncoder[motorC] > -i) && (nMotorEncoder[motorB] <i ) ) //while the encoder wheel turns one revolution
{
// This condition waits for motors B + C to come to an idle position. Both motors stop
// and then jumps out of the loop
}
int x=(i/angle)-1;
left2[x]=SensorValue(sleft);
right2[x]=SensorValue(sright);
}
motor[motorB] = 0;
motor[motorC] = 0;
}
// Zjistovani bloku za jizdy po leve strane
task zmerBlokVlevo(){
if(!rozhliziSe){
seeLeft = SensorValue(sonar);
if(seeLeft < 30){
wait1Msec(50);
seeLeft = SensorValue(sonar);
}
if(seeLeft < 30 && BlokBludisteRelativne(-1, 0) != 1) BlokBludisteRelativne(-1, 0, 255);
//else if(BlokBludisteRelativne(-1, 0) == 0) BlokBludisteRelativne(-1, 0, 2);
nxtDisplayCenteredTextLine(0, "L: %d R: %d", seeLeft, seeRight);
}
}
