Gear Placement Code

[Kyle Sa]

C++ Team
Here is some pseudo-code for my idea for the gear placement. I will try to come up with the other parts of this after.

Code Select Expand


while placeGear==true {
        if (reflectCorrect==true) {
                 if (ultrasonicDistance==0) {
                         letGo();
                         return(0);
                 }else{
                         moveForwardGear(1);
                 }
         }else{
                 alignGear();
         }
}

Explanation: The "placeGear" variable is a boolean, and is used to see whether or not we want to start the program or not (probably would put this in a function of some sort that listens for user input). "reflectCorrect" is also a boolean, and checks to see whether the robot is properly aligned with the gear. This would use an infared sensor/reciever against the retroreflective tape to make sure the robot is lined up correctly (this would most likely be used in an outside function, and would be called before checking the variable. However, I didn't put this in the pseudo-code in case there would be a better way to do this, or if the gear-checker would automatically run every so often by itself. "ultrasonicDistance" is a float/double, and would use the ultrasonic sensor to check the distance between it and the gear area (the value could be adjusted, depending on where the ultrasonic sensor is located on the robot, and would probably end up being a range of some sort). The robot would let go of the gear, if it is at the right distance, using a"letGo()" function. If it wasn't the right distance, it would move forward a certain amount using the "moveForwardGear" function. The argument "1" is just a placeholder. In reality, this would probably be a specified number of milliseconds, based on the distance needed to travel (this would probably be found using an algorithm based on the distance from the airship. The loop would then end (using return, if that is correct), since the gear would therefore be in the right place. If the robot is in the correct position, but farther away, the robot would then move forward a designated amount (not too much, in case the robot wasn't completely aligned), and then go through the loop again to once again check to see if it is in the right position, and repeat. Finally, if the robot wasn't in the right position in the first place, it would go through the function "alignGear();", and then would repeat the function for finding the distance away.

This is just my idea for the code; if anyone on the C++ team has any suggestions for improving this, or turning it into code, please reply to this. Also, if anyone has  ideas for the smaller functions within this broad one, please post it as well.

[Kyle Sa]

C++ Team "moveForwardGear" code from previous post. Not sure if this is correct, but I believe it should work. Also, not sure how the motor speeds work, so for this, I will assume that motor speed "1" is the fastest, and motor speed "0" is stopped.

Code Select Expand


void Drivetrain::moveForward(float timeTravel) {
          Drivetrain::driveForwardRight(0; 0)
          Drivetrain::driveForwardLeft(0; 0)
          Drivetrain::driveBackRight(0; 0)
          Drivetrain::driveBackLeft(0; 0)         
          //The previous was just to set the rotation straight, the next is to move the robot.
          Drivetrain::driveForwardRight(0.25; 0)
          Drivetrain::driveForwardLeft(0.25; 0)
          Drivetrain::driveBackRight(0.25; 0)
          Drivetrain::driveBackLeft(0.25; 0)
          //Some way to make this wait for "timeTravel" milliseconds, I don't know how to do this, so here is the replacement.
          Drivetrain::stopDrive();
}

Notes: the variables for "driveForwardRight" are already in our code, but just to reiterate: the first one is the speed, the second is the rotation value. (Not sure if I got the rotation value right, but it is meant to turn the wheels forward in the first call of each function, and not rotate in the second. The rotation value, for this code's purpose, is the direction at which it is pointing; I may be completely wrong on how this part works, but this is just to make sure the wheels are pointing straight towards the airship). This code is very crude; I ran out of time, but I believe this still should work. Again, this is just an idea on how we could do this, and if anyone who knows more about how to correctly do this, please post it.

[Matt S]

Most of these values take some kind of threshold/margin of error into account.

Code Select Expand


while(GearIsPlacing)
{
        IgnoreUserinput();
static double yaw = 0; //stores yaw position of robot for easy repositioning
if(GetCalcIR = 1000) //Error value that GetCalcIR returns if no sensors are triggered
{
GearIsPlacing = false;
return;
}
if(GetUltrasonicLeft >= 3000 OR GetUltrasonicRight >= 3000)
{
//Do nothing
GearPlacing = 0;
return;
}
else if(GetUltrasonicLeft > GetUltrasonic)
{
RotateRight();
}
else if(GetUltrasonicLeft < GetUltrasonicRight)
{
RotateLeft();
}
else if(GetUltrasonicLeft <= 609.6 AND GetUltrasonicRight <= 609.6) //609.6 is 2 feet in mm, just a minimum distance we chose
{
if(GetCalcIR < 0)  //GetCalcIR calculates the distance the robot has to strafe to be able to line up with the lift
{
MoveRight(0.1); //0.1 is the speed
}
else if(GetCalcIR > 0)
{
MoveLeft(0.1);
}
else if(GetCalcIR = 0)
{
yaw = GetGyroYaw();
if(GetUltrasonic >= 50.8) //Minimum distance away from wall , 2in
{
MoveForward(0.2);
}
else if(GetUltrasonic <= 50.8)
{
StopMovement();
ReleaseGear();
MoveBackward(0.2); //We are moving backward to helpout the driveteam
SetTimeout(0.5); //Is in seconds
                                StopMovement();
GearIsPlacing = false;
return;
}
}
}
}


[Vidula K]

Gear placement pseudo code:

This is assuming that the robot is parallel to the lift

Code Select Expand


Loop {
     If (only sensor 3) {
     Move right
     }
     If (only sensor 2) {
     Move right
     }
     If (sensors 1 and 3) {
     Move right
     }
     If (no sensors) {
     Move right a little bit
          If (still no sensors) {
          Move left until detects sensors 1 and 2
          }
     }
     If (sensors 1 and 2) {
     Go Forward
     Stop Loop
     }
}



Here is an idea for rangefinders to make the robot parallel with the lift:

Two rangefinders on each side of the robot, continue turning in one direction until the two rangefinders return equal distances. if the robot is close enough to the lift to hit it, move a short distance back and keep turning until the rangefinders return equal values.

[Kyle Sa]

C++ Team
Pixy Camera Integration Psuedo-Code:
This is just some psuedo-code for using the values given by the Pixy Camera to align the gear. Not sure if the following is completely correct, or the most efficient idea, but I think it could work.

After making sure that both targets are in the camera view, the rest will follow:
Notes: Packet 1 is the target on the left, and Packet 2 is the target on the right

First, make sure the targets are the correct ones for the gear (there is a margin of error), checking both packets

Code Select Expand


if height!=5 || width!=2 {
printf("error");
}



Check if both packets' y values are equal, then if the y values are not equal (with a margin of error), output this to the user, and/or adjust the code accordingly
   

Code Select Expand


        float poleY;
poleY=(yPacket1+yPacket2)/2; /*This is just to accomodate for the margin of error; if they are slightly off from each other, then the average will be around the correct value*/
        float poleX;


Find center of the 2 x's in order to find the pole where the gear needs to be placed
   

Code Select Expand

poleX = (xPacket1+xPacket2)/2

Now the coordinates are found for where the pole is, this following sets the center of where the gear in the robot is
   

Code Select Expand


        float centerX=320  //Note, these two items are constants, so they can be changed easily in case that
float centerY=200  //these values are incorrect. This is currently just the center of the camera.


This part compares the robot's center with the coordinates of the pole

Code Select Expand


int failSafe= 10; //10 is just an placeholder for whatever the value would be (see below comment for what failSafe is)
for (int x=0; int x<failSafe ;x++) {  //failSafe is whatever value is needed to make sure the loop doesn't go on forever if
      //there is an error and the robot is never aligned correctly; a failsafe

if (poleX>=(centerX-errorMarginX) && poleX<=(centerX+errorMarginX)) {         //errorMarginX is just the X margin of
                              //error allowed when placing the gear.
if (poleY>=(centerY-errorMarginY) && poleX<=(centerY+errorMarginX)) { //errorMarginY is the same as above, but for Y
x=failSafe
moveForward(); //Function to move the robot forward after making sure the robot is aligned
}else{
fixY(offYPixel); //Function to fix the Y position. The argument "offYPixel" is the pixels off of where it needs to be
}else if (poleY>=(centerY-errorMarginY) && poleX<=(centerY+errorMarginX)) {
fixX(offXPixel); //Function to fix the X position. The argument "offXPixel" is the pixels off of where it needs to be
}else{
fixY(offYPixel);
fixX(offXPixel);
}

}


Using the pixels off from above, this translates it into rotations of the wheels (or inches, speed & milliseconds, or whatever is needed
to move the robot)

Code Select Expand


float offX= offXPixel*(translateX) //translateX is the fraction: (inchesPerPixel/1pixel), to create a value in inches (or whatever else would be needed)
float offY= offYPixel*(translateY) //translateY is the same as above, but with Y

The code for fixing the X and Y is not in here yet
   

[Louis L]

I'm behind on catching up on the posts. Here are some thoughts.

   
• Please note that regardless of what language we program in, the procedures we use should be the same. So it's not a C++ thing, it's for all languages even if the pseudocode is written in C++.
  
• As I understand it (as of this writing), the redesigned Gear mechanism will have 4 servos and 2 rangefinders.
  
• The servos will act in pairs with one pair opening doors to let the Gear out and the other pair to actually push the Gear out.
  
• The ultrasonic HC-SR04 rangefinders operate from 2 cm to 400 cm (3/4 inch to over 13 feet)
  
• We should assume that in autonomous mode, the drivetrain will get us "close enough". If not, there's really nothing we can do. In teleop mode, the driver is responsible for getting us close enough.
  
• Once close enough, there are 6 degrees of motion to consider - 3 are rotations (yaw, pitch, roll) and 3 are translations (x,y,z movement). As a robot on a carpet, we can reduce this to just 3 degrees of motion - yaw (whether we're parallel to the face of the airship), x and y translation (moving on the carpet in 2 dimensions).
  
• We can rotate and translate together or separately. Either way, it's just as if the user had entered commands via the joysticks (one joystick to translate x, y and the other to rotate).
  
• We'll be using a camera (pixy presumably) to get our bearing. There are limitations to what the camera can do but the closer we get the more accurate the data should be. Be sure you know the field-of-view of the camera - 47 degrees vertical and 75 degrees horizontal. This can help you with reaching your rotation alignment quickly.

[Louis L]

We had some more small discussions about stuff today so I'm going to write down my thoughts on what I think the algorithm should be. Feel free to critique it.

First some assumptions and basis.

   
• Assume that the robot will get us "close enough".
• Note that the horizontal field of view is 75 degrees - that's like having a 24mm lens on a 35mm camera - pretty wide angle!
• Therefore, if the pixy can't pick up the vision targets, we should probably just punt. At the very least we should ignore this situation until later (if we have time to work on it).
• The pixy is mounted below the Gear mechanism and should have a decent view of the target. For now, assume that if we don't see both targets, we're not close to our Lift. Again, we can ignore this situation until later.

So here's what I have in mind as an overall procedural list of how things can work. Note that this is layered, so that we can always add more conditions up-front. The back-end is where we want to focus our initial efforts (it may be all we care about too).

   
• Get robot close enough using gyro and times drive commands.
• If pixy sees 0 targets, punt (can amend later)
• If pixy sees 1 target, punt (can amend later) (we differentiate 0 from 1 because of potential severity of misalignment)
• If pixy sees 2 targets of appropriate relative size and distance (within some error margin) then go to AlignmentPhase1
• TBD - if we were to read a 3rd object, and there was no 3rd object, what would we get back? (the first or zeros?

AlignmentPhase1 (assumption: we see 2 targets)

   
• Goal is to align the robot to be parallel to the airship, with the Lift aimed at the center of the Gear at a distance of 14 inches (14 is arbitrary at this point). The reason for doing this at a distance is to avoid hitting the Lift spring.
• Use rangefinder to get left and right distance.
• Calculate angular deviation. If angle is larger than +/- 30 degrees, we have a problem (reading is outside the margin for the rangefinder) so we punt.
• Using the rangefinder values, determine the Y translation necessary to move us to the point 14 inches from the airship.
• Find the horizontal midpoint between the two targets and using the field of view, calculate the X translation needed to center the Lift within the field of view.
• Sent the X & Y translation and yaw rotation to the drivetrain, moving at a low speed (value TBD).

Repeat steps above until X,Y and yaw values are within some small error margin. Then move in and release gear.
AlignmentPhase2

   
• Goal is to move in Y direction only towards Lift. So we do the same thing as above but now we move in towards Lift.
• Use rangefinder to get left and right distance.
• Calculate angular deviation. At this point the deviation should be tiny.
• Using the rangefinder values, determine the Y translation necessary to move us to the point 3.25 inches from the airship (depth of bumpers).
• Find the horizontal midpoint between the two targets and using the field of view, calculate the X translation needed to center the Lift within the field of view. This should be negligible and within the error margin.
• Sent the X & Y translation and yaw rotation to the drivetrain, moving at a low speed (value TBD)

Once we've reached our goal, we release the Gear.

   
• Open gates
• Push out Gear
• Drive backwards in Y, until the rangefinders say we've pulled back enough to be clear of the Lift spring.
• Retract Gear pusher
• Close gates

You can probably see some common pieces of code that get re-used. Work on those first. Geometry to follow in upcoming post.

[Shreya C]

Pseudocode for alignment

Code Select Expand

Assuming that you have 2 distinct packets that are the appropriate size (with a given error margin):
AlignmentPhase1() {
targetY = 14
repeat until (leftDistance, rightDistance, angularDevitation w/in certain error margin)
leftDistance = rangeFinderGetLeftDistance();
rightDistance = rangeFinderGetRightDistance();
angularDeviation = arcsin((leftDistance - rightDistance)/width);  //width = width of chassis
if (angularDeviation > 30 deg || angularDeviation < -30 deg) discardData();
else {
yTranslation = targetY - rangeFinderGetLeftDistance(); //could be rightDistance too, they should be same
xTranslation = abs(packet1XCoord - packet2XCoord)/2;
sendToDriveTrain(xTranslation, yTranslation, angularDeviation);

AlignmentPhase2() — repeat AlignmentPhase1() but targetY = 3.5 not 14

releaseGear()
pushOutGear()
repeat until (leftDistance > 14) {
driveBackwardsY()
}
retractGearPusher()
closeGates()