![Picture](/uploads/2/8/1/5/28153289/6455387_orig.jpg)
As you know, I have been working so hard on the new strategy making of Skylark. Here is the raw versionn of the new autonomous program we have
#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, 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;
}*/
}