254D Cheesy Poofs High-Hang Description

dontworryaboutit · April 18, 2011, 3:02pm

First of all, please check out the video we made on Saturday to showcase the robot and see what I’m actually going to be talking about here:
Team 254D's High Hanging Robot - YouTube (:40 and 1:37)
Here’s a whole match: http://www.youtube.com/watch?v=4wGvrBH2jgo

Intro
Let me start by congratulating 1103 on winning robot skills and having one of the best robots at the competition. I love their hanging system, specifically how it locks onto the ladder. Also, 44 and all the weeble-dumpers did a great job with a very unique strategy that proved to be quite a challenge to compete against.
At Orlando this past weekend, 254D was the second pick for the seed 1 team in the Technology Division (677). The reason for this was not just the overall functionality of the robot, but its ability to high-hang reliably and quickly. 254D also had 97 points in robot skills after a fairly bad performance and successful high-hang.
Many teams asked us about how it actually achieved the high-hang, and all were impressed by its speed, so now I’ll take some time to explain how it actually hangs in under 2 seconds.

The Hook
The hook is designed to be lightweight, strong, and easy to use.
Main features:
-The hook itself is two 12.5" aluminum c-channels and two two-inch standoffs to hook onto the side of the ladder (over the top green bar):
Hook (side) -
-The release holds the hook into the robot until the arm is raised for the first time, at which point the release folds up and stays there:
Set release (side) -
Set release (top) -
After released (top) -
-The support structure is two standoffs that prevent the robot from being pushed to the side while hanging, two cut rails that support the robot while hanging, and the whole intake in general (which is very sturdy):
Intake (top-front) -

The Ratchet
The ratchet keeps the robot from falling once motor is cut at the end of the round.
Main features:
-The ratchet itself consists of two 12-tooth high-strength gears that are pulled into the two 12-tooth high-strength gears that power the bottom of the arm lift gears:
Lift gears (back) -
-The firing mechanism consists of a Kit-2 piston that pulls the ratchet into its place while hanging and a zip tie and rubber band that keep the ratchet from accidentally getting pulled into place during the match (See Note 1):
Ratchet (back-bottom)

The Pneumatics
The pneumatics are really what puts it all together by activating the ratchet, removing the arm support rubber bands, and providing additional power to the hang.
Main features:
-The pneumatics themselves consist of one tank, 2 Kit-1 pistons and 2 Kit-2 pistons (See Note 2), and one solenoid that fires all the pistons right before hanging:
Pneumatics system and motors (front) -
-The rubber band-release piston (See Note 2) removes the rubber bands that keep the arm buoyant during the match (they pull the arm up, but for hanging the arm needs to go down):
Rubber bands (side) -
Release piston (side-bottom) - (it’s the lower piston)
-The hang-assisting pistons give a little extra help to the hang by countering what the arm-support rubber bands did (See Note 2):
Pistons (side) - (same picture as Rubber bands (side))
Pistons (bottom-side) - [ because, despite being 2:1 on 4" wheels (being fast), it really is not that good. We were being pushed sideways and had a hard time getting traction when the arm was down.

I’d like to thank you for your interest in other robot designs and I wish you luck in the 2011-2012 season.

The MotorsSee Note 4Other Notable Featuresclawarmdrive baseNotes:
1: We used another solenoid to push the ratchet piston out at the beginning of the match (it needs to start in so we stay under 18"), but removed it because it was experiencing strange problems (firing whenever we turned the robot on-we tried changing the ports, solenoids, and fitting and running a blank code, but to no avail).
2: We use Kit-2 pistons instead of Kit-1 on the rubber band-release piston because we only received two pneumatics kits: one Kit-1 and one Kit-2.
3: Pistons apply 12 pounds of force (a little less for the Kit-2 action that pulls the piston in) over 2 inches, so 2 pistons=4 foot-pounds of energy. 254D’s hang-assisting pistons actually only fire about 1.7 inches to give them enough spare room to keep from being used as hard stops (you should be cringing right now), but the robot only moves vertically about 18.3", so the 2 hang-assisting pistons effectively reduce the weight of the robot by 2 pounds. The energy the motors need to output is therefore equal to 18.3" x ( weight of robot - weight of intake - .5 x weight of arm ).
4: 254D used fully-charged batteries for each match (about 8.1 volts) to get the most power into the robot. Before one match, the batteries were not replaced and it took about 4 seconds to hang, a huge difference from the standard 2 seconds. Use new batteries.](http://img709.imageshack.us/img709/5193/img1541b.jpg (same picture as Release piston (side-bottom)))

allengregoryiv · April 18, 2011, 3:47pm

This is really awesome, thank you for positing this.

I hope more teams post details of their robots.

jgraber · April 18, 2011, 9:59pm

Thanks for posting the details.
Can you add some pictures of whole robot side view, with arm up and with arm down? I can’t see the whole elephant from these pictures.
Can you add a concise motor allocation list? looks like…

2x393+2x269 on arm to lift and to hang
1?x269 to operate clamp claw (nice claw)
2x393+2x269 for 2:1 (speed) on 4" wheels (?) drive (4 wheel drive?)

I’m collecting programming ideas, so it would be great if you could post your code.
Titan (1103) said he plays to post his this week.

Had you considered these ideas?

Move the 4 arms motors to the base and power the arm with chain + sprocket.
(lower CG is better)
hinges instead of axles for unpowered 4-bar arm joints (less side play, fits your 5wide C channel arm-bar concept well)
a 1-shot, spring-powered hang that wouldn’t require the locking thingys
H slide drive: it looks like several times in the video that it would have helped alignment.
Goal alignment cup, so when you bump the goal, the robot is in the right place to score.
a lower arm descore fork, or a lower arm goal lifter.

Kendalls · April 18, 2011, 10:34pm

At one point in time 254D had a 1-shot rubber band powered hanging mechanism. It was pretty much 50 rubber bands pulling down on a linear slider.

They abandoned the idea because of the stress it was putting on a lot of the robot and because if it was accidentally fired during a match there would be no coming back from that.

It was also pretty dangerous.

dontworryaboutit · April 18, 2011, 11:05pm

Yeah here are some pictures (the blue paper is to add contrast, not make it look better):
Arm down (right) - ImageShack - Best place for all of your image hosting and image sharing needs
Arm up (right) -
Body (left) -
Back -

As for the motors:
2 393 + 2 269 on arm
2 3-wire on claw (we only needed 1, but having 9 motors is ugly)
2 393 + 2 3-wire on drive (2:1 with 4" omnis, only driving front)
And here’s how we used the ports to make the most out of those 4-amp limiters:
port1, lArmBott
port2, rDrive1 (power expander)
port3, rDrive2 (power expander)
port4, intake
port5, lArmTop
port6, rArmTop
port7, intake2
port8, lDrive1 (power expander)
port9, lDrive2 (power expander)
port10, rArmBott

I only have an older program, but I’m sure when our programmer reads this, he’ll post the new code.

We had actually considered chaining the arm for a while, but I can’t remember the exact reason why we didn’t do that. Here are what I think some of the reasons were:
Chain has more slop than gears
It gets in the way of the Cortex
Sprockets are bigger than gears
It would require work
Gears look cool

I’m not sure what the 4-bar arm joint is, but I think it’s the arm attachment assembly on the tower. I don’t really like hinges, just because they are harder to attach than axles (axles can go across the tower, where hinges need cross-pieces) and the center of rotation is weird. We had little side play with the arm as it was.

Over Christmas break I actually built a whole mechanism that used rubber bands and a linear slider to hang, but we decided over the course of a few months that it was a little too sketchy (and it took about 20 minutes to reload). We also bent the lower aluminum c-channel during a test (it’s now steel).
Here’s an early picture of the proof-of-concept hanging with rubber bands (before the linear slider mechanism, which did work eventually, was built):
[http://img857.imageshack.us/img857/268/img1343gm.jpg (it is actually hanging, but the flash on the camera was on so you can’t see the shadow- look at the wheel height and the position of the arm compared to the box height)
As we built up the pneumatic system, starting with the rubber band release, we thought about using only pneumatics to hang. But, that would require 8 single-acting pistons or some kind of stepped thing. The ratchet is pretty cool as well.

I really wanted to use that extra motor to put in a strafing wheel, but we didn’t really have the time at Orlando (we finished building Thursday), and I thought that practice would make it better anyway, which it did. The real solution to the maneuvering issue was to either move the hook down 1/2" and allow the hook to move out another 1/4" (the issue you can see in the video was the standoffs tending to rest on the metal parts of the ladder instead of sliding behind them, so that 1/4" would make it 4x more successful the first time) or to make a hook that went around the sides of the ladder instead of over the top. I don’t know why we never did the latter, but it was probably because we only had the hanging mechanism done about two weeks before Orlando.

I spent quite a while trying to make a goal alignment thing, but the arm kept getting in the way. It would have been very easy to make something that “hugged” the edge of the weeble at the right distance, but once again we only had a few days of practicing before Orlando.

We saw a video of a separate arm descorer, but the standoffs on the bottom of the intake worked very well for descoring (if only the preset height button worked well…). With our drive base design, it would have been difficult to incorporate a weeble-lifter that does not cause the robot to fall over forwards while holding a weeble. We could have put a hook on the bottom of the intake/arm, but then the intake would be in the way while trying to put it under the ladder.

The common theme here is that we simply did not have enough time to practice and perfect the system.

Thank you for your feedback, we really appreciate it!](http://img857.imageshack.us/img857/268/img1343gm.jpg (it is actually hanging, but the flash on the camera was on so you can’t see the shadow- look at the wheel height and the position of the arm compared to the box height))

jgraber · April 19, 2011, 9:22am

Thanks for the new pictures, there’s lots of good detail there.
I particularly like img835, showing the 1/2" standoffs for the width-brace C.

It looks like you have front-wheel drive only?
If thats the case, consider changing to non-omni in front.
Tital 1103 also mentioned improved performance with 6 wheels over 4.

Drive rails are 1x5x1 C on the inside, plus an outer steel plate.
Is the outer plate only tied to the inner with hexbars at the top?
No, I see one bottom hexbar tie in img222. Just the one?
Extending the width brace to the outer drive rail would be a good idea.

What are the pillow block bearings on the front bumpers and top rear tower for?

Re chain vs gear for the arm, you implementation is simple and robust, but has a high CG. Motors at the bottom really help CG. Your centered drive motors are a good example. You could have also mounted them rotated 90 degrees (flat-mode) and down 2-3 holes. As an example of what might be possible, consider that double-catapult-concept-robot had 4 drive motors and 4 arm motors all mounted in flat mode with centerlines < 1.5" from floor.

You might experiment on a proto chassis with two stage reduction:

a 1:3 or 1:5 gear at the arm joint (reduces the chain slack issue by 1:N)
with the 1 gear on a 18t HS sprocket, powered by a 6tHS sprocket on a motor shaft in the base, for a total of 9 or 15 ratio, (other ratios possible)

You mention desiring a narrow tower. If you cut pockets in the 1x5x1 lower arm for the large gear, and mounted them inside, you could have made the tower narrower by 2-4 holes.

The Cortex is low with Vexnet key low and pointing down, surrounded by metal framing. Did you have any Vexnet connection issues? I have mixed results with an 18" gold-plated USB extension cable from Monoprice.com

For finishing the robot so late, you performed very well.
What do you think is your biggest contributor to that?

general past experience with similar designs
skilled drivers
good execution of a good strategy
other misc luck issues (good qualifying alliance partners vs bad opponents)

dontworryaboutit · April 19, 2011, 10:09am

We had initially used 4" non-omnis in the front and 4" omnis in the back, with the front driven, but for some reason we couldn’t turn. Instead of trying to solve that problem, we just decided to use all omnis, as we were running out of time and all omnis was a quick and easy fix.
We needed to remove as much weight as possible from the drive base (for hanging), so we settled on a 4-wheel drive.
The outer plate is aluminum. There are three standoffs on the top and two on the bottom (one is missing (you can see where the screw was) in img835 because we were halfway through moving it when we were knocked out of semis, and another one is behind the nameplate).
The straps in the front make up for the lack of a standoff, and we never had time to reinforce the back.
We have polycarb pieces that fit over the sides of the drive base (and have a notch for the width brace c-channel) to protect the gears and give extra support (as well as looking cool). We never kept it on because we were changing things so often. The standoffs provided ample support for the outside of the drive base, and the strap running across the bottom of the drive base has the duel purpose of keeping the drive base from deforming and blocking the rings from getting stuck under the robot (it was in the wrong place in semis, so we got stuck on a ring and could not maneuver to hang-we were behind 15 points, so hanging would have put us into finals in the Tech division ).

On the front bumpers, they prevent the robot from falling over forwards (without them, when you start to fall over, the wheels just roll backwards and you keep going over). The pillow blocks on the back are for mounting a standoff-and-strap ring holder for scoring on the wall post in autonomous. It can be put on either side. The pillow blocks on the arm/tower joint keep that top axle from bending (those were added Saturday).

The robot’s CG is fairly far forward (especially when the arm is raised), and because only the front wheels are driven, we found it impossible to knock the robot over backwards. We wouldn’t fall over sideways either because everything is in the middle.
When we were geared 1:5 with only 2 393s on the arm (there were 6 3-wires on the drive), the CG seemed sufficiently low.
Mounting the motors sideways would be hard because we are using a 2x2 hole L and we can’t mount pillows on the back for the pivot because the gears would go out of the 18".

There was not enough room for mounting the motors lower, as the Cortex needs to be there to be accessible with the arm down. I was thinking about doing 2 1:3 reductions for the arm, or mounting two motors on the arm that powered a gear that was fixed to the tower, but those were never implemented due to the complexity of the change.

In late March (our last major redesign before Orlando), we widened the tower by 1", added the extra motors to the arm (from the drive base), and changed the gearing (we had 1 1:5 gearing, which we changed to the current 2 1:7 gearings). The reason for the widening was to allow for both 84-tooth double-gears. It also made it easier to put in the spacing and plug wires into the Cortex. I don’t think cutting pockets in the 1x5 c-channel would have been structurally sound. It would have also created problems with the gears skipping (notice the 1/4-pitch holding the lower powered axle to the geared arm axle to prevent skipping) if you are saying what I think you are saying (which is move the gears inside the c-channel). We had no problems with the width of the tower as it is now.

We had no Vexnet issues with the USB keys. If anything, it protected the key from getting knocked around. When we used a USB A-A cable to run the robot, we were getting a bad connection, but I think that was the cable.

I think the biggest contributors to our success were looking at other robots (on youtube, other 254 teams, and at tournaments (specifically Pan-Pacific)) and having experience with building and driving with VEX. Our driver is one of the best on 254, and the code was designed to make it easier for him (for example, when the joysticks are moved, the motor power jumps right to the lowest level that will make the robot go, so there is no lag when trying to maneuver at low speeds).
We also received help from many other 254 teams, specifically 254A, who told us what would work when it was built and what would not.
We like to call “254D” “254Doesn’t at competitions” because whenever we took it to competitions, something wouldn’t work. At our home Bellarmine tournament, our Cortex died and we had to use a PIC that we pulled off another robot. Then, the drive wouldn’t work and we had severe Vexnet connection issues. We generally had bad luck in everything. Also at Orlando, in the 3 qual matches we lost, 2 were because our alliance partner wasn’t working and the other 1 was because our drive battery was not plugged in (I can’t wait for next year’s tournaments!). And in robot skills, we missed going on Thursday by 5 minutes, and in our 97-point run on Friday, we dropped 3 rings. Those 3 rings pushed us from 8th place into 12th and out of getting another round on Saturday.
Round-Up was a great game because of its simplicity, and we were great at that because there were few moving parts, and hanging came naturally after having a large arm.

jgraber · April 19, 2011, 11:04am

I forgot to mention, re powerexpander, and forgetting to plug it in.
Consider changing the power expander assignments to 2 for wheels and 2 for arms.
That way, if the power expander fails (to be plugged in), you might still be able to drive and move arms. It also spreads the driving load when you are just driving, and spreads the load for hanging when you are just moving the arm.

dontworryaboutit · April 19, 2011, 11:29am

This is very true!
We only got as far as balancing the Cortex this year, but next year we’ll be spreading everything out. The battery not being plugged in issue won’t happen again due to the change in the rules, but spreading the current around will help to keep the limiters from limiting.
The reason we did not start to mix drive base and arm motors with Cortex and Motor controller power was that it was not a problem. Hanging worked well, and driving worked well, so there was no need. But, every little thing helps!

BellAnimator · April 19, 2011, 6:25pm

Here is the code. It’s a little sloppy, due to the fact that most of the original, neat code was changed to accommodate the many physical changes on the robot. I had to delete the pragmas and the single driver code so that I did not exceed the line limit for forum posts.


/////////////////////////////////CONSTANTS////////////////////////////////////
#define LOW 412
#define HIGH 954
#define HIGHEST 1295
#define LOWEST 219
#define NINETYDEG 0
#define LEFT 1
#define RIGHT -1
#define DESCORE 330
/////////////////////////////////////////////////////////////////////////////

////////////////////////////////GLOBAL VARIABLES///////////////////////////
int step=0;
bool pistonEngaged = false;
bool opAutoEnabled=false;
bool buttonPressed=false;
bool buttonReleased=false;
int opAutoDesiredHeight=330;
int wheelDiameter=4;
int minScaledPower = 40;
int lDrive;
int rDrive;
//////////////////////////////////////////////////////////////////////////

////////////////////////////////FUNCTIONS////////////////////////////////
void setRightDrive(int pwm);
void setLeftDrive(int pwm);
void setArm(int height);
void clearEncoders();
void driveStraight(int dir, int sp, int dist);
void driveStraight(int dir, int sp);
void turn(int dir, int sp, int r);
void stopDrive();
int logDrive(int rawSpeed);
float distanceToEncoderVal(float distance);
/////////////////////////////////////////////////////////////////////////

task autonomous()
{
  clearEncoders();
  switch(step)
  {
    case 0:
      setArm(LOW);
      ClearTimer(T1);
      while(abs(SensorValue[lEncoder])<(360*2.5) && time1[T1] < 4000)
        driveStraight(1, 100);
      stopDrive();
    /*case 0:
      setArm(LOW);
      motor[intake]=127;
      motor[intake2]=127;
      wait1Msec(300);
      motor[intake]=0;
      motor[intake2]=0;
      setArm(LOWEST);
      driveStraight(1,100,180);
      stopDrive();
      ClearTimer(T1);
      while(SensorValue[range_sensor]>9 && time1(T1) < 4000)
      {
        setLeftDrive(-70);
        setRightDrive(70);
      }
      stopDrive();
      while(SensorValue[range_sensor] > 4)
        driveStraight(1,60);
      stopDrive();
      //driveStraight(1,60,10);
      stopDrive();
      step++;
    case 1:
      motor[intake]=127;
      motor[intake2]=127;
      wait1Msec(1200);
      motor[intake]=0;
      motor[intake]=0;
      //driveStraight(1,100,distanceToEncoderVal(3));
      stopDrive();
      clearEncoders();
      step++;
    case 2:
      setArm(HIGH);
      turn(LEFT, 80, 360*2);
      driveStraight(1, 100, 360*3);
      stopDrive();
      motor[intake]=motor[intake2]=70;
      wait1Msec(700);
      motor[intake]=motor[intake2]=0;
    case 2:
      //setArm(HIGH);
      driveStraight(1,600,200);
      step++;
    case 3:
      if(SensorValue[jumper1]==1)
        turn(RIGHT,100,151); //old constant 220
      else
        turn(LEFT,100,45);
      clearEncoders();
      step++;
    case 4:
      while(abs(SensorValue[lEncoder])<179) //old constant 260
        driveStraight(1,80);
      stopDrive();
      clearEncoders();
      step++;
   case 5:
      setArm(LOW);
      step++;
    case 6:
      motor[intake]=-127;
      motor[intake2]=-127;
      wait1Msec(1000);
      motor[intake]=0;
      motor[intake]=0;
      step++;
    case 7:
      motor[rDrive1]=motor[rDrive2]=motor[lDrive1]=motor[lDrive2]=-127;
      wait1Msec(700);
      motor[rDrive1]=motor[rDrive2]=motor[lDrive1]=motor[lDrive2]=0;
  */}
}

task usercontrol()
{
 {
    while(true)
    {
    //r=2, l=3
    if(vexRT[Ch2] >0 && vexRT[Ch2]<=5) rDrive=0;
    else if(vexRT[Ch2]>5 && vexRT[Ch2]<=64) rDrive = vexRT[Ch2]+10;
    else if(vexRT[Ch2]>64 && vexRT[Ch2]<=85) rDrive = 74+18;
    else if(vexRT[Ch2]>85 && vexRT[Ch2]<=106) rDrive = 74+18+18;
    else if(vexRT[Ch2]>106 && vexRT[Ch2]<=127) rDrive = 74+18+18+18;
    else if(vexRT[Ch2]<-10 && vexRT[Ch2]>=-64) rDrive = vexRT[Ch2]-10;
    else if(vexRT[Ch2]<-64 && vexRT[Ch2]>=-85) rDrive = -74-18;
    else if(vexRT[Ch2]<-85 && vexRT[Ch2]>=-106) rDrive = -74-18-18;
    else if(vexRT[Ch2]<-106 && vexRT[Ch2]>=-127) rDrive = -74-18-18-18;

    if(vexRT[Ch3] >0 && vexRT[Ch3]<5) lDrive=0;
    else if(vexRT[Ch3]>5 && vexRT[Ch3]<=64) lDrive = vexRT[Ch3]+10;
    else if(vexRT[Ch3]>64 && vexRT[Ch3]<=85) lDrive = 74+18;
    else if(vexRT[Ch3]>85 && vexRT[Ch3]<=106) lDrive = 74+18+18;
    else if(vexRT[Ch3]>106 && vexRT[Ch3]<=127) lDrive = 74+18+18+18;
    else if(vexRT[Ch3]<0 && vexRT[Ch3]>=-64) lDrive = vexRT[Ch3]-10;
    else if(vexRT[Ch3]<-64 && vexRT[Ch3]>=-85) lDrive = -74-18;
    else if(vexRT[Ch3]<-85 && vexRT[Ch3]>=-106) lDrive = -74-18-18;
    else if(vexRT[Ch3]<-106 && vexRT[Ch3]>=-127) lDrive = -74-18-18-18;

    motor[rDrive1]=motor[rDrive2]=rDrive;
    motor[lDrive1]=motor[lDrive2]=lDrive;
    //claw motor, controlled by operator
    motor[intake]=motor[intake2]=-vexRT[Ch2Xmtr2];

    //fires hanging piston
    if(vexRT[Btn6UXmtr2]==1) SensorValue[ratchet_piston]=1;
    else if(vexRT[Btn6DXmtr2]==1) SensorValue[ratchet_piston]=0;

    if(vexRT[Btn5UXmtr2]==1&& vexRT[Btn5U]==1 && !pistonEngaged){
      SensorValue[arm_release]=1;
      pistonEngaged=true;
    }
    else if(vexRT[Btn5DXmtr2]==1&& vexRT[Btn5D]==1 && pistonEngaged)
    {
      SensorValue[arm_release]=0;
      pistonEngaged=false;
    }
    if(vexRT[Btn7DXmtr2] == 1 && !opAutoEnabled)
    {
      wait1Msec(30);
      if(vexRT[Btn7DXmtr2] == 0)
        opAutoEnabled=true;

    }
    else if(vexRT[Btn7DXmtr2] == 1 && opAutoEnabled)
    {
      wait1Msec(30);
      if(vexRT[Btn7DXmtr2] == 0)
        opAutoEnabled=false;

    }
    switch(opAutoEnabled)
    {
      case true:
		    if(SensorValue[armPot]<DESCORE-20)
		      motor[lArmTop]=motor[lArmBott]=motor[rArmTop]=motor[rArmBott]=30;
		    else if(SensorValue[armPot]>DESCORE+20)
		      motor[lArmTop]=motor[lArmBott]=motor[rArmTop]=motor[rArmBott]=-30;
			  else if (SensorValue[armPot]<DESCORE+20 && SensorValue[armPot]>DESCORE-20)
			  {
			    motor[lArmTop]=motor[lArmBott]=motor[rArmTop]=motor[rArmBott]=0;
			  }
			  break;
			case false:
			  motor[lArmTop]=motor[lArmBott]=motor[rArmTop]=motor[rArmBott]=vexRT[Ch3Xmtr2];
			  break;
	}

  }
}
}


void setArm(int height){
  if(SensorValue(armPot)>height){
    while(SensorValue(armPot)>height+3){
      motor[lArmTop]=motor[lArmBott]=motor[rArmTop]=motor[rArmBott]=-90;
    }
    motor[lArmTop]=motor[lArmBott]=motor[rArmTop]=motor[rArmBott]=0;
  }
  else if(SensorValue(armPot)<height){
    while(SensorValue(armPot)<height-3){
      motor[lArmTop]=motor[lArmBott]=motor[rArmTop]=motor[rArmBott]=90;
    }
    motor[lArmTop]=motor[lArmBott]=motor[rArmTop]=motor[rArmBott]=0;
  }
}

void clearEncoders(){
  SensorValue[rEncoder]=0;
  SensorValue[lEncoder]=0;
}

void setRightDrive(int pwm){
  motor[rDrive1]=pwm;
  motor[rDrive2]=motor[rDrive1];
}

void setLeftDrive(int pwm){
  motor[lDrive1]=pwm;
  motor[lDrive2]=motor[lDrive1];
}

void driveStraight(int dir, int sp, int dist){
  while(abs(SensorValue(lEncoder))<dist)
  {
		if(abs(SensorValue(rEncoder))>abs(SensorValue(lEncoder))){
			setLeftDrive(sp+(dir*20));
			setRightDrive(sp);
		}
		else if(abs(SensorValue(rEncoder))<abs(SensorValue(lEncoder))){
			setLeftDrive(sp);
			setRightDrive(sp+(dir*20));
		}
		else{
			setLeftDrive(sp);
			setRightDrive(sp);
		}
  }
  stopDrive();
}

void driveStraight(int dir, int sp)
{
  if(abs(SensorValue(rEncoder))>abs(SensorValue(lEncoder))){
   setLeftDrive(sp+(dir*20));
  setRightDrive(sp);
  }
  else if(abs(SensorValue(rEncoder))<abs(SensorValue(lEncoder))){
    setLeftDrive(sp);
    setRightDrive(sp+(dir*20));
  }
  else{
    setLeftDrive(sp);
    setRightDrive(sp);
  }
}

void stopDrive(){
  motor[lDrive1]=motor[lDrive2]=motor[rDrive1]=motor[rDrive2]=0;
}

int logDrive(int rawSpeed){
  int dir=(rawSpeed>=0)?1:-1;
  int logSpeed=((rawSpeed*rawSpeed)/(dir*127));
  return logSpeed;
}

void turn(int dir, int sp, int r){
  if(dir==LEFT)
  {
    while(abs(SensorValue[rEncoder])<r){
      motor[lDrive1]=motor[lDrive2]=(-dir)*sp;
      motor[rDrive1]=motor[rDrive2]=dir*sp;
    }
  }
  else if (dir == RIGHT)
  {
    while(abs(SensorValue[lEncoder])<r){
      motor[lDrive1]=motor[lDrive2]=(-dir)*sp;
      motor[rDrive1]=motor[rDrive2]=dir*sp;
    }
  }
  stopDrive();
}

float distanceToEncoderVal(float distance)
{
  float encoderVal = (distance/(wheelDiameter*3.1415))*90;
  return (int) encoderVal;
}

BellAnimator · April 26, 2011, 5:01pm

Code’s a little messy because most of the robot was coded on the fly.


task autonomous()
{
  clearEncoders();
  switch(step)
  {
    /*case 0:
      setArm(LOW);
      ClearTimer(T1);
      while(abs(SensorValue[lEncoder])<(360*2.5) && time1[T1] < 4000)
        driveStraight(1, 100);
      stopDrive();*/
    case 0:
      setArm(LOW);
      motor[intake]=127;
      motor[intake2]=127;
      wait1Msec(300);
      motor[intake]=0;
      motor[intake2]=0;
      setArm(LOWEST);
      driveStraight(1,100,180);
      stopDrive();
      ClearTimer(T1);
      while(SensorValue[range_sensor]>9 && time1(T1) < 4000)
      {
        setLeftDrive(-70);
        setRightDrive(70);
      }
      stopDrive();
      while(SensorValue[range_sensor] > 4)
        driveStraight(1,60);
      stopDrive();
      //driveStraight(1,60,10);
      stopDrive();
      step++;
    case 1:
      motor[intake]=127;
      motor[intake2]=127;
      wait1Msec(1200);
      motor[intake]=0;
      motor[intake]=0;
      //driveStraight(1,100,distanceToEncoderVal(3));
      stopDrive();
      clearEncoders();
      step++;
    case 2:
      setArm(HIGH);
      turn(LEFT, 80, 360*2);
      driveStraight(1, 100, 360*3);
      stopDrive();
      motor[intake]=motor[intake2]=70;
      wait1Msec(700);
      motor[intake]=motor[intake2]=0;
    case 2:
      //setArm(HIGH);
      driveStraight(1,600,200);
      step++;
    case 3:
      if(SensorValue[jumper1]==1)
        turn(RIGHT,100,151); //old constant 220
      else
        turn(LEFT,100,45);
      clearEncoders();
      step++;
    case 4:
      while(abs(SensorValue[lEncoder])<179) //old constant 260
        driveStraight(1,80);
      stopDrive();
      clearEncoders();
      step++;
   case 5:
      setArm(LOW);
      step++;
    case 6:
      motor[intake]=-127;
      motor[intake2]=-127;
      wait1Msec(1000);
      motor[intake]=0;
      motor[intake]=0;
      step++;
    case 7:
      motor[rDrive1]=motor[rDrive2]=motor[lDrive1]=motor[lDrive2]=-127;
      wait1Msec(700);
      motor[rDrive1]=motor[rDrive2]=motor[lDrive1]=motor[lDrive2]=0;
  }
}

task usercontrol()
{
  
  if(SensorValue[jumper2]==0)
  {
    while(true)
	  {
	    //Logarithmic drive gives better control in extreme ranges
	    int RawSpeedR=vexRT[Ch2];
	    int RawSpeedL=vexRT[Ch3];
	    int LogSpeedR=logDrive(RawSpeedR);
	    int LogSpeedL=logDrive(RawSpeedL);
	    motor[rDrive1]=motor[rDrive2]=LogSpeedR;
	    motor[lDrive1]=motor[lDrive2]=LogSpeedL;

	    if(vexRT[Btn8U]==1 && !opAutoEnabled && vexRT[Btn5U]==0 && vexRT[Btn5D]==0)
	      opAutoEnabled = true;
	    else if(vexRT[Btn8U]==1 && opAutoEnabled)
	      opAutoEnabled = false;
	    //claw motor, controlled by operator
	    switch(opAutoEnabled)
	    {
	      case false:
	        if(vexRT[Btn5U]==1)
	            motor[lArmTop]=motor[lArmBott]=motor[rArmTop]=motor[rArmBott]=127;
	        else if(vexRT[Btn5D]==1)
	            motor[lArmTop]=motor[lArmBott]=motor[rArmTop]=motor[rArmBott]=-127;
	        else
	            motor[lArmTop]=motor[lArmBott]=motor[rArmTop]=motor[rArmBott]=0;
	        break;
	      case true:
	        motor[lArmTop]=motor[lArmBott]=motor[rArmTop]=motor[rArmBott]=30;
	        break;
	      default:
	        opAutoEnabled=false;
	        break;
	    }
	    //descore preset
	    if(vexRT[Btn7U]==1)
	    {
	      while(SensorValue[armPot]<DESCORE)
	        motor[lArmTop]=motor[lArmBott]=motor[rArmTop]=motor[rArmBott]=127;
	      motor[lArmTop]=motor[lArmBott]=motor[rArmTop]=motor[rArmBott]=0;
	    }
	    if(vexRT[Btn6U]==1)
	        motor[intake]=motor[intake2]=127;
	    else if(vexRT[Btn6D]==1)
	        motor[intake]=motor[intake2]=-127;
	    else
	        motor[intake]=motor[intake2]=0;

	    //fires hanging pBtn8U
	    if(vexRT[Btn8R]==1) SensorValue[ratchet_piston] = 1;
	    if(vexRT[Btn8L]==1) SensorValue[ratchet_piston] = 0;

	    if(vexRT[Btn7U]==1 && vexRT[Btn8U]==1 && !pistonEngaged){
	      SensorValue[arm_release]=1;
	      pistonEngaged=true;
	    }
	    else if(vexRT[Btn7D]==1 && vexRT[Btn8D]==1 && pistonEngaged){
	      SensorValue[arm_release]=0;
	      pistonEngaged=false;
	    }
	  }
  }
  else if(SensorValue[jumper2]==1)
  {
    while(true)
    {
    //r=2, l=3
    if(vexRT[Ch2] >0 && vexRT[Ch2]<=5) rDrive=0;
    else if(vexRT[Ch2]>5 && vexRT[Ch2]<=64) rDrive = vexRT[Ch2]+10;
    else if(vexRT[Ch2]>64 && vexRT[Ch2]<=85) rDrive = 74+18;
    else if(vexRT[Ch2]>85 && vexRT[Ch2]<=106) rDrive = 74+18+18;
    else if(vexRT[Ch2]>106 && vexRT[Ch2]<=127) rDrive = 74+18+18+18;
    else if(vexRT[Ch2]<-10 && vexRT[Ch2]>=-64) rDrive = vexRT[Ch2]-10;
    else if(vexRT[Ch2]<-64 && vexRT[Ch2]>=-85) rDrive = -74-18;
    else if(vexRT[Ch2]<-85 && vexRT[Ch2]>=-106) rDrive = -74-18-18;
    else if(vexRT[Ch2]<-106 && vexRT[Ch2]>=-127) rDrive = -74-18-18-18;

    if(vexRT[Ch3] >0 && vexRT[Ch3]<5) lDrive=0;
    else if(vexRT[Ch3]>5 && vexRT[Ch3]<=64) lDrive = vexRT[Ch3]+10;
    else if(vexRT[Ch3]>64 && vexRT[Ch3]<=85) lDrive = 74+18;
    else if(vexRT[Ch3]>85 && vexRT[Ch3]<=106) lDrive = 74+18+18;
    else if(vexRT[Ch3]>106 && vexRT[Ch3]<=127) lDrive = 74+18+18+18;
    else if(vexRT[Ch3]<0 && vexRT[Ch3]>=-64) lDrive = vexRT[Ch3]-10;
    else if(vexRT[Ch3]<-64 && vexRT[Ch3]>=-85) lDrive = -74-18;
    else if(vexRT[Ch3]<-85 && vexRT[Ch3]>=-106) lDrive = -74-18-18;
    else if(vexRT[Ch3]<-106 && vexRT[Ch3]>=-127) lDrive = -74-18-18-18;

    motor[rDrive1]=motor[rDrive2]=rDrive;
    motor[lDrive1]=motor[lDrive2]=lDrive;
    //claw motor, controlled by operator
    motor[intake]=motor[intake2]=-vexRT[Ch2Xmtr2];

    //fires hanging piston
    if(vexRT[Btn6UXmtr2]==1) SensorValue[ratchet_piston]=1;
    else if(vexRT[Btn6DXmtr2]==1) SensorValue[ratchet_piston]=0;

    if(vexRT[Btn5UXmtr2]==1&& vexRT[Btn5U]==1 && !pistonEngaged){
      SensorValue[arm_release]=1;
      pistonEngaged=true;
    }
    else if(vexRT[Btn5DXmtr2]==1&& vexRT[Btn5D]==1 && pistonEngaged)
    {
      SensorValue[arm_release]=0;
      pistonEngaged=false;
    }
    if(vexRT[Btn7DXmtr2] == 1 && !opAutoEnabled)
    {
      wait1Msec(30);
      if(vexRT[Btn7DXmtr2] == 0)
        opAutoEnabled=true;

    }
    else if(vexRT[Btn7DXmtr2] == 1 && opAutoEnabled)
    {
      wait1Msec(30);
      if(vexRT[Btn7DXmtr2] == 0)
        opAutoEnabled=false;

    }
    switch(opAutoEnabled)
    {
      case true:
		    if(SensorValue[armPot]<DESCORE-20)
		      motor[lArmTop]=motor[lArmBott]=motor[rArmTop]=motor[rArmBott]=30;
		    else if(SensorValue[armPot]>DESCORE+20)
		      motor[lArmTop]=motor[lArmBott]=motor[rArmTop]=motor[rArmBott]=-30;
			  else if (SensorValue[armPot]<DESCORE+20 && SensorValue[armPot]>DESCORE-20)
			  {
			    motor[lArmTop]=motor[lArmBott]=motor[rArmTop]=motor[rArmBott]=0;
			  }
			  break;
			case false:
			  motor[lArmTop]=motor[lArmBott]=motor[rArmTop]=motor[rArmBott]=vexRT[Ch3Xmtr2];
			  break;
	}

  }
}
}