Hey all!
This is my first attempt at writing some code for our autonomous (wow! Were getting off to a late start (::looks at calendar:
4 days til ship! :ahh: )
Anyway, I’d like everyone to browse through and offer some comments.
Now, this isn’t the best code in the world, I mainly tried to writeup everything into one packet, so some stuff might be messy, and I know I can remove a few variables, but I wrote them in for neatness’s sake.
Here it is!:
//Init all our Variables
int time=0; //define a counter in the beginning
int seconds=0; //set up a variable for the time in seconds too!
int step=0; //Define our steps too!
int tracking = 0; //Define our tracking watcher
int tea_time = 0; //A temporary time holder :) Its Tea time!
int start_pos = 0; //Field startup Position
int left_sw_hist = 0; //Left Switch History
int rght_sw_hist = 0; //Right Switch History
int fwd_spd = 175; //Tracking forward speed
//Maintain our start position data from the RC
if (rc_dig_in11 == 1)
{
start_pos = 1;
}
if (rc_dig_in12 == 1)
{
start_pos = 2;
}
time++; //Increment the time!
seconds = time / 1000; //Get the time in seconds!
//Reset all drive/arm variables
pwm11 = pwm12 = 127;
relay2_fwd = relay3_fwd = relay4_fwd = 0;
//Set up the Pneumatics to keep the charge
relay1_fwd = !rc_dig_in01; //Run the pump if the system is low on pressure.//
relay1_rev =0;
if (step == 0) //Drive out onto the field a bit
{
pwm11 = pwm12 == 150;
if (time == 500)
{
step++;
}
}
if(step==1) //Begin the track sequence, track the tetra until contact
{
camera_init(45,19,121); //Init the Camera with the calibration values
camera_auto_servo(1); //Turn on the Auto Servo Feature
camera_find_color(GREEN); //Seek Green
if (rc_dig_in02 == 1 || rc_dig_in03 == 1) //If one of the skirt switches trip, go to the next step.
{
if (rc_dig_in02 == 1) //Record Left History for the next step
{
left_sw_hist = 1;
}
if (rc_dig_in03 == 1) //Record right history for the next step
{
rght_sw_hist = 1;
}
tea_time = time;
step++;
}
}
if (step==2) //Align the tetra with the "grabber"
{
if (left_sw_hist == 1 && rght_sw_hist == 1)
{
left_sw_hist = 0;
rght_sw_hist = 0;
step++;
}
if (left_sw_hist == 0 && rght_sw_hist == 1)
{
pwm11 = 175;
pwm12 = 130;
if (time == tea_time + 150)
{
left_sw_hist = 0;
rght_sw_hist = 0;
step++;
}
}
if (left_sw_hist ==1 && rght_sw_hist == 0)
{
pwm11 = 130;
pwm12 = 175;
if (time == tea_time + 150)
{
left_sw_hist = 0;
rght_sw_hist = 0;
step++;
}
}
}
if (step==3) //Raise the arm to loading stand height and check to see if the tetra is held. If so, proceed. Otherwise, retrack the tetra.
{
relay2_fwd = relay3_fwd = 1; //extend the arm
if (rc_dig_in06 == 1)
{
if (rc_dig_in04 == 1)
{
step++;
}
if (rc_dig_in04 == 0)
{
relay2_fwd = relay3_fwd = 0;
relay4_fwd = 1;
if (rc_dig_in05 == 1)
{
step = 1;
}
}
}
}
if (step==4) //Track up to the goal until contact is made
{
camera_find_color(YELLOW); //Seek Yellow
relay2_fwd = relay3_fwd = 1; //extend the arm
if (rc_dig_in10 == 1)
{
relay2_fwd = relay3_fwd = 0; //Stop extending the piston if its fully extended
}
if (rc_dig_in02 == 1 || rc_dig_in03 == 1) //If one of the skirt switches trip, go to the next step.
{
if (rc_dig_in02 ==1) //Record Left History for the next step
{
left_sw_hist = 1;
}
if (rc_dig_in03 ==1) //Record right history for the next step
{
rght_sw_hist = 1;
}
tea_time = time;
step++;
}
}
if (step==5) //Almost done! Wait until the last few seconds to trigger the capping sequence!
{
if (left_sw_hist == 1 && rght_sw_hist == 1)
{
pwm11 = pwm12 = 130;
if (seconds == 13) //Lets try giving it 2 seconds to play it safe for now...
{
step++;
}
}
if (left_sw_hist == 1 && rght_sw_hist == 0)
{
pwm11 = 130;
pwm12 = 140;
if (rc_dig_in03 == 1)
{
rght_sw_hist = 1;
}
}
if (left_sw_hist == 0 && rght_sw_hist == 1)
{
pwm11 = 140;
pwm12 = 130;
if (rc_dig_in02 == 1)
{
left_sw_hist = 1;
}
}
}
if (step==6) //The Final step! Plant that tetra on the goal in the last few seconds!
{
relay4_fwd = 1;
}
if(camera_track_update()==1)
{
if (cam.size>0)
{
tracking=1;
}
else
tracking=0;
}
if (step==1 || step==4) //If we're in a tracking step, drive!
{
if(tracking==1) //Follow the color if we see any of it!
{
pwm11 = Limit_Mix(2000 + fwd_spd + cam.pan_servo - 127);// Ok now put some single joystick drive code here using the variable cam.pan_servo as the x axis of the joystick, and a constant value as the y axis
pwm12 = Limit_Mix(2000 + fwd_spd - cam.pan_servo + 127);
}
else
pwm11 = 63; //Seek to the left if all else fails!
pwm12 = 175;
if (start_pos == 1) //If we started on the left half of the field, turn to the right
{
pwm11 = 145;
pwm12 = 100;
}
if (start_pos == 2) //If we started on the right half of the field, turn to the left
{
pwm11 = 100;
pwm12 = 145;
}
}
I used some tidbits of code posted here on CD to start with. Now, I just wrote this (at 1am) so I won’t get to test it til tomorrow, but I’m crossing my fingers that it’ll work ok! It did compile!!!
Any comments would be great. Just understand I’m not making it neat or extremely well laid out just yet, I’m trying to get the main process down
Thanks!