Skylark is not doing very well today. The Teleop program did not run properly like I expected and the autonomous program was caught in trouble because of the back pusher. My plan right now is to redo most of the adjusting test and see what we can do . Here is the new tele op program that I have wrote.
#pragma config(Hubs, S1, HTMotor, HTMotor, HTMotor, none)
#pragma config(Hubs, S2, HTServo, none, none, none)
#pragma config(Sensor, S3, IRSensor, sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S4, USSensor, sensorSONAR)
#pragma config(Motor, motorA, Garm, tmotorNXT, PIDControl, encoder)
#pragma config(Motor, mtr_S1_C1_1, motorleft, tmotorTetrix, openLoop, reversed, driveLeft, encoder)
#pragma config(Motor, mtr_S1_C1_2, motorright, tmotorTetrix, openLoop, driveRight, encoder)
#pragma config(Motor, mtr_S1_C2_1, arm, tmotorTetrix, openLoop, driveLeft, encoder)
#pragma config(Motor, mtr_S1_C2_2, sweeping, tmotorTetrix, openLoop, reversed)
#pragma config(Motor, mtr_S1_C3_1, upperlift, tmotorTetrix, openLoop)
#pragma config(Motor, mtr_S1_C3_2, lowerlift, tmotorTetrix, openLoop)
#pragma config(Servo, srvo_S2_C1_1, pullarm, tServoStandard)
#pragma config(Servo, srvo_S2_C1_2, cylinder, tServoStandard)
#pragma config(Servo, srvo_S2_C1_3, rightpull, tServoStandard)
#pragma config(Servo, srvo_S2_C1_4, leftpull, tServoStandard)
#pragma config(Servo, srvo_S2_C1_5, servo5, tServoNone)
#pragma config(Servo, srvo_S2_C1_6, servo6, tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
#include "JoystickDriver.c"
task main()
{
//waitForStart();
servo[pullarm] = 255;
servo[cylinder] = 3;
servo[leftpull]= 225;
servo[rightpull] = 0;
nMotorEncoder[motorleft] = 0; // arm up
nMotorEncoder[motorright] = 0;
motor[lowerlift] = 55;
wait1Msec(2600);
motor[lowerlift] = 0;
motor[arm] = -55;
wait1Msec(500);
motor[arm] = 0;
while (nMotorEncoder[motorleft] < 9000) // drive off ramp
{
motor[motorleft] = 80;
motor[motorright] = 75;
}
nMotorEncoder[motorleft] = 0;
nMotorEncoder[motorright] = 0;
while (nMotorEncoder[motorleft] < 1500)//smooth touch the 60cm
{
motor[motorleft] = 55;
motor[motorright] = 55;
}
nMotorEncoder[motorleft] = 0;
nMotorEncoder[motorright] = 0;
while (nMotorEncoder[motorleft] > -550) // back up for scoring
{
motor[motorleft] = -55;
motor[motorright] = -55;
}
motor[motorleft] = 0;
motor[motorright] = 0;
wait1Msec(1000);//releasing gate
servo[cylinder] = 225;
wait1Msec(1000);
servo[cylinder] = 3;
wait1Msec(1000);
nMotorEncoder[motorleft] = 0; // back up
nMotorEncoder[motorright] = 0;
while (nMotorEncoder[motorleft] > -500)
{
motor[motorleft] = -55;
motor[motorright] = -55;
}
nMotorEncoder[motorleft] = 0;
nMotorEncoder[motorright] = 0;
while (nMotorEncoder[motorright] < 3700)// turn around
{
motor[motorleft] = -55;
motor[motorright] = 55;
}
motor[motorleft] = 0;
motor[motorright] = 0;
motor[arm] = 55; // arm back down
wait1Msec(550);
motor[arm] = 0;
motor[lowerlift] = -55;
wait1Msec(2600);
motor[lowerlift] = 0;
nMotorEncoder[motorleft] = 0; // back up
nMotorEncoder[motorright] = 0;
/*while (nMotorEncoder[motorleft] > -800)
{
motor[motorleft] = -55;
motor[motorright] = -55;
}
nMotorEncoder[motorleft] = 0; // back up for aattach
nMotorEncoder[motorright] = 0;*/
while (nMotorEncoder[motorleft] > -2700)
{
motor[motorleft] = -25;
motor[motorright] = -25;
}
motor[motorleft] = 0;
motor[motorright] = 0;
wait1Msec(1000); // main pull down
servo[pullarm] =200;
wait1Msec(1000);
nMotorEncoder[motorleft] = 0;
nMotorEncoder[motorright] = 0;
while (nMotorEncoder[motorleft] < 6500)// turn before sensor
{
motor[motorleft] = 75;
motor[motorright] = -15;
}
/*while(USSensor > 40)//sensor turn
{
motor[motorleft] = 55;
motor[motorright] = 0;
}*/
nMotorEncoder[motorleft] = 0;
nMotorEncoder[motorright] = 0;
while (nMotorEncoder[motorleft] < 500)//sensor forward
{
motor[motorleft] = 40;
motor[motorright] = 40;
}
wait1Msec(300);
nMotorEncoder[Garm] = 0; //Garm down
while (nMotorEncoder[Garm] < 130)
{
motor[Garm] = 75;
}
wait1Msec(300);
nMotorEncoder[Garm] = 0; // Garm back up/*
/*while (nMotorEncoder[Garm] > -130)
{
motor[Garm] = -75;
}
servo[leftpull]= 180;//leftpull down
wait1Msec(300);
nMotorEncoder[motorleft] = 0;
nMotorEncoder[motorright] = 0;
while (nMotorEncoder[motorleft] > -300)// Back Up
{
motor[motorleft] = -55;
motor[motorright] = -55;
}
nMotorEncoder[motorleft] = 0; // turn for 90cm
nMotorEncoder[motorright] = 0;
while (nMotorEncoder[motorleft] < 1500)
{
motor[motorleft] = 75;
motor[motorright] = 5;
}
nMotorEncoder[motorleft] = 0; // forward for 90cm
nMotorEncoder[motorright] = 0;
while (nMotorEncoder[motorleft] < 700)
{
motor[motorleft] = 55;
motor[motorright] = 55;
}
wait1Msec(300); // rightpull down
servo[rightpull] = 180;
wait1Msec(300);
nMotorEncoder[motorleft] = 0;
nMotorEncoder[motorright] = 0;
while (nMotorEncoder[motorleft] > -3000)// Back Up
{
motor[motorleft] = -75;
motor[motorright] = -75;
}
nMotorEncoder[motorleft] = 0;
nMotorEncoder[motorright] = 0;
while (nMotorEncoder[motorleft] > -2000)// turn
{
motor[motorleft] = -75;
motor[motorright] = 75;
}
nMotorEncoder[motorleft] = 0;
nMotorEncoder[motorright] = 0;
while (nMotorEncoder[motorleft] > -14000)// Back Up
{
motor[motorleft] = -75;
motor[motorright] = -75;
}
nMotorEncoder[motorleft] = 0;
nMotorEncoder[motorright] = 0;
while (nMotorEncoder[motorright] > -2000)// turn
{
motor[motorleft] = 75;
motor[motorright] = -75;
}
nMotorEncoder[motorleft] = 0;
nMotorEncoder[motorright] = 0;
while (nMotorEncoder[motorleft] > -1000)// Back Up
{
motor[motorleft] = -75;
motor[motorright] = -75;
}*/
}
#pragma config(Hubs, S2, HTServo, none, none, none)
#pragma config(Sensor, S3, IRSensor, sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S4, USSensor, sensorSONAR)
#pragma config(Motor, motorA, Garm, tmotorNXT, PIDControl, encoder)
#pragma config(Motor, mtr_S1_C1_1, motorleft, tmotorTetrix, openLoop, reversed, driveLeft, encoder)
#pragma config(Motor, mtr_S1_C1_2, motorright, tmotorTetrix, openLoop, driveRight, encoder)
#pragma config(Motor, mtr_S1_C2_1, arm, tmotorTetrix, openLoop, driveLeft, encoder)
#pragma config(Motor, mtr_S1_C2_2, sweeping, tmotorTetrix, openLoop, reversed)
#pragma config(Motor, mtr_S1_C3_1, upperlift, tmotorTetrix, openLoop)
#pragma config(Motor, mtr_S1_C3_2, lowerlift, tmotorTetrix, openLoop)
#pragma config(Servo, srvo_S2_C1_1, pullarm, tServoStandard)
#pragma config(Servo, srvo_S2_C1_2, cylinder, tServoStandard)
#pragma config(Servo, srvo_S2_C1_3, rightpull, tServoStandard)
#pragma config(Servo, srvo_S2_C1_4, leftpull, tServoStandard)
#pragma config(Servo, srvo_S2_C1_5, servo5, tServoNone)
#pragma config(Servo, srvo_S2_C1_6, servo6, tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
#include "JoystickDriver.c"
task main()
{
//waitForStart();
servo[pullarm] = 255;
servo[cylinder] = 3;
servo[leftpull]= 225;
servo[rightpull] = 0;
nMotorEncoder[motorleft] = 0; // arm up
nMotorEncoder[motorright] = 0;
motor[lowerlift] = 55;
wait1Msec(2600);
motor[lowerlift] = 0;
motor[arm] = -55;
wait1Msec(500);
motor[arm] = 0;
while (nMotorEncoder[motorleft] < 9000) // drive off ramp
{
motor[motorleft] = 80;
motor[motorright] = 75;
}
nMotorEncoder[motorleft] = 0;
nMotorEncoder[motorright] = 0;
while (nMotorEncoder[motorleft] < 1500)//smooth touch the 60cm
{
motor[motorleft] = 55;
motor[motorright] = 55;
}
nMotorEncoder[motorleft] = 0;
nMotorEncoder[motorright] = 0;
while (nMotorEncoder[motorleft] > -550) // back up for scoring
{
motor[motorleft] = -55;
motor[motorright] = -55;
}
motor[motorleft] = 0;
motor[motorright] = 0;
wait1Msec(1000);//releasing gate
servo[cylinder] = 225;
wait1Msec(1000);
servo[cylinder] = 3;
wait1Msec(1000);
nMotorEncoder[motorleft] = 0; // back up
nMotorEncoder[motorright] = 0;
while (nMotorEncoder[motorleft] > -500)
{
motor[motorleft] = -55;
motor[motorright] = -55;
}
nMotorEncoder[motorleft] = 0;
nMotorEncoder[motorright] = 0;
while (nMotorEncoder[motorright] < 3700)// turn around
{
motor[motorleft] = -55;
motor[motorright] = 55;
}
motor[motorleft] = 0;
motor[motorright] = 0;
motor[arm] = 55; // arm back down
wait1Msec(550);
motor[arm] = 0;
motor[lowerlift] = -55;
wait1Msec(2600);
motor[lowerlift] = 0;
nMotorEncoder[motorleft] = 0; // back up
nMotorEncoder[motorright] = 0;
/*while (nMotorEncoder[motorleft] > -800)
{
motor[motorleft] = -55;
motor[motorright] = -55;
}
nMotorEncoder[motorleft] = 0; // back up for aattach
nMotorEncoder[motorright] = 0;*/
while (nMotorEncoder[motorleft] > -2700)
{
motor[motorleft] = -25;
motor[motorright] = -25;
}
motor[motorleft] = 0;
motor[motorright] = 0;
wait1Msec(1000); // main pull down
servo[pullarm] =200;
wait1Msec(1000);
nMotorEncoder[motorleft] = 0;
nMotorEncoder[motorright] = 0;
while (nMotorEncoder[motorleft] < 6500)// turn before sensor
{
motor[motorleft] = 75;
motor[motorright] = -15;
}
/*while(USSensor > 40)//sensor turn
{
motor[motorleft] = 55;
motor[motorright] = 0;
}*/
nMotorEncoder[motorleft] = 0;
nMotorEncoder[motorright] = 0;
while (nMotorEncoder[motorleft] < 500)//sensor forward
{
motor[motorleft] = 40;
motor[motorright] = 40;
}
wait1Msec(300);
nMotorEncoder[Garm] = 0; //Garm down
while (nMotorEncoder[Garm] < 130)
{
motor[Garm] = 75;
}
wait1Msec(300);
nMotorEncoder[Garm] = 0; // Garm back up/*
/*while (nMotorEncoder[Garm] > -130)
{
motor[Garm] = -75;
}
servo[leftpull]= 180;//leftpull down
wait1Msec(300);
nMotorEncoder[motorleft] = 0;
nMotorEncoder[motorright] = 0;
while (nMotorEncoder[motorleft] > -300)// Back Up
{
motor[motorleft] = -55;
motor[motorright] = -55;
}
nMotorEncoder[motorleft] = 0; // turn for 90cm
nMotorEncoder[motorright] = 0;
while (nMotorEncoder[motorleft] < 1500)
{
motor[motorleft] = 75;
motor[motorright] = 5;
}
nMotorEncoder[motorleft] = 0; // forward for 90cm
nMotorEncoder[motorright] = 0;
while (nMotorEncoder[motorleft] < 700)
{
motor[motorleft] = 55;
motor[motorright] = 55;
}
wait1Msec(300); // rightpull down
servo[rightpull] = 180;
wait1Msec(300);
nMotorEncoder[motorleft] = 0;
nMotorEncoder[motorright] = 0;
while (nMotorEncoder[motorleft] > -3000)// Back Up
{
motor[motorleft] = -75;
motor[motorright] = -75;
}
nMotorEncoder[motorleft] = 0;
nMotorEncoder[motorright] = 0;
while (nMotorEncoder[motorleft] > -2000)// turn
{
motor[motorleft] = -75;
motor[motorright] = 75;
}
nMotorEncoder[motorleft] = 0;
nMotorEncoder[motorright] = 0;
while (nMotorEncoder[motorleft] > -14000)// Back Up
{
motor[motorleft] = -75;
motor[motorright] = -75;
}
nMotorEncoder[motorleft] = 0;
nMotorEncoder[motorright] = 0;
while (nMotorEncoder[motorright] > -2000)// turn
{
motor[motorleft] = 75;
motor[motorright] = -75;
}
nMotorEncoder[motorleft] = 0;
nMotorEncoder[motorright] = 0;
while (nMotorEncoder[motorleft] > -1000)// Back Up
{
motor[motorleft] = -75;
motor[motorright] = -75;
}*/
}