Chief Delphi - Encoders and gyro

Quote:

Originally Posted by tomy (Post 1024023)

So i have two questions for you people on CD

1) When we put in our encoder code it stops the robot from driving, why?

here is what we do

Code:

Encoder en1;

         DigitalInput *en1a;

         DigitalInput *en1b; 

     

         en1(en1a,en1b);

 

         en1a = new DigitalInput(6);

         enba = new DigitalInput(6);



         en1.Reset();

         printf("encoder =%f\n",  encoder.GetDistante());

The second question is this:
When we drive our robot in mecanum drive with the gyro it works ok but a few glitches:
strafe left works
strafe right turns 180 degrees and then strafes left....
rotation dosent really rotate just swings the back end of the robot left or right....

any ideas?

Your creation of the encoder is kind of scrambled, you don't need to create Digital Inputs for the Encoder, the Encoder class understands DIs already.

Here's a complete robot program with an encoder. It has limit switches and a pot used for limits as well. I wrote it for the WRRF pre-season worshops here in the Silicon Valley and it still has the Watchdog in it.

Code:

/* Demo program for WRRF workshops Dec 11, 2010. Basic cRIO C++ program for driving

** and controlling one arm, and an extendable boom. This robot has four motors:

** There is one drive motor per side (left and right), one motor for raising the arm,

** and one motor for extending/retracting the boom.

**

** Each motor is controlled by a "Victor" motor controller. These controllers take 

** instructions from our program, running on the cRIO, and drive the motors fast and

** slow as well as reversing the direction of the motor. "Jaguars" are the newer version

** of the motor controllers, and we could have used them. This robot is from the 2005 FRC

** game "Triple Threat" and it still has Victors.

** 

** Our program will take its inputs from the two joysticks and drive the four motors

** accordingly. One joystick controls the wheel motors (left and right). The joystick works

** in Arcade style, Y deflection makes the 'bot go forwards/backwards, X deflection makes the

** robot turn.

**

** Y deflection of the arm joysick controls the arm motor to move the arm up and down. It's 

** hinged at its base and the motor actually turns a winch to lift the far end of the

** boom up. As the winch unwinds, gravity pulls the arm back down. A potentiometer at the

** hinge turns as the arm angle increases and decreases, telling us the angle of the arm.

** Pots are analog devices (give a continuously variable value, like a volume control

** knob, or the temperature knob of an oven.)

**

** X deflection of the arm joystick extends and retracts the boom, which is inside the

** arm. The boom motor is fitted to a garage-door jackscrew and it's used to

** extend and retract the arm. At the base of the arm there's a mechanical switch that

** is pushed as the boom comes close to the end of its mechanical travel. There's an

** optical sensor(swtich) that detects when the boom is extended to the limit.

** These switches are digital devices: they are either on or off.

**

** Our program has to take input from ("read") the pot and the switches and turn off the 

** motors before the arm/boom extend past the end of their mechanical travel. If the

** program didn't do this the motors would keep going and the robot would be damaged

** and/or the motors overloaded, blow their fuse or overheat and be damaged.

**

** All four motors are controlled by PWM (Pulse-Width Modulation) outputs from the cRIO.

** You don't need to know what PWM really means, just that each motor has its own PWM

** output.

**

** There are several analog inputs, digital inputs, and PWM outputs available on the

** cRIO, and each is numbered. 

*/



/* these are the various constants used in the program. They are all here, as #defines,

** so they can be easily found, identified, and changed as necessary.

** #defines are simple. The compiler (on WindRiver Workbench) sees the text and substitutes

** the number. For example, "DRIVE_JOYSTICK" that appears in the program causes a "1" to

** be substituted in its place - simple as that.

*/



/* you can tell which joystick is which: on the Drivers Station, click on Setup, and

** watch "Joystick Setup" as you pull the trigger(s) of the Joysticks. You can drag the

** order of the JS(s).

*/



#define DRIVE_JOYSTICK 1 // the joysticks are numbered by the Classmate Drivers station.

#define ARM_JOYSTICK   2 // assignments can be seen with the Drivers Station "Setup" tab.



#define LEFT_MOTOR_PWM      2

#define RIGHT_MOTOR_PWM     3 

#define ARM_WINCH_MOTOR_PWM 5

#define BOOM_MOTOR_PWM      6



// identifiers for the sensors.

// boom limit switches are wired to Digital Inputs:



#define BOOM_LIMIT_SWITCH_DI_LOW 10

#define BOOM_LIMIT_SWITCH_DI_HIGH 2



// encoder digital inputs:



#define ENCODER_DI_A 8

#define ENCODER_DI_B 9

#define ENCODER_RESET_BUTTON 2





// Potentiometers ("pots") are wired to Analog Inputs:

#define ARM_POT_ANALOG_IN         3



#define ARM_POT_LIMIT_HIGH  950

#define ARM_POT_LIMIT_LOW   790

#define ARM_SLOW_RATIO      0.5



// how many loops pass between our debug prints:

#define PRINT_COUNT 10000 



#define AUTON_DRIVE_SPEED  -0.5 // half speed

#define AUTON_TURN   0.0 // no turn, go straight 

#define AUTON_ARM_DOWN_SPEED -0.5 // negative for down



#include "WPILib.h"



class RobotDemo : public SimpleRobot

  {



  RobotDrive *myRobot; // WPILib has given us easy-to-use drive software system.



  Joystick *drive_stick; // this joystick will control the driving

  Joystick *arm_stick; // Y deflection will raise the arm up and down, X deflection

  // will extend/retract the boom.



  Victor *arm_motor, *boom_motor;



  AnalogChannel *arm_pot;



  DigitalInput *boom_limit_retract, *boom_limit_extend;



  Encoder *koder;



public:

  RobotDemo(void)

    {



      // Create the WPILib robot drive software subsystem:

      myRobot = new RobotDrive(LEFT_MOTOR_PWM, RIGHT_MOTOR_PWM);



      /* these statemets tell RobotDrive if the direction of the motor(s)

       ** need to be reversed in order to make the robot drive properly.

       ** The direction of the motor turning depends on how they are geared

       ** and wired.

       */



      myRobot->SetInvertedMotor(RobotDrive::kFrontLeftMotor, false);

      myRobot->SetInvertedMotor(RobotDrive::kFrontRightMotor, false);



      // the following turn out to be our two motors. We just changed the "false" to "true"

      // one by one and watched how the motors turned, to find this out. Leaving the

      // above two statements in doesn't hurt anything.



      myRobot->SetInvertedMotor(RobotDrive::kRearLeftMotor, true);

      myRobot->SetInvertedMotor(RobotDrive::kRearRightMotor, true);



      drive_stick = new Joystick(DRIVE_JOYSTICK);

      arm_stick = new Joystick(ARM_JOYSTICK);



      arm_motor = new Victor(ARM_WINCH_MOTOR_PWM); // + moves it up

      boom_motor = new Victor(BOOM_MOTOR_PWM); // + pulls it in



      arm_pot = new AnalogChannel(ARM_POT_ANALOG_IN); // 950 at top, 

      // 790 at bottom



      boom_limit_retract = new DigitalInput(BOOM_LIMIT_SWITCH_DI_LOW);

      boom_limit_extend = new DigitalInput(BOOM_LIMIT_SWITCH_DI_HIGH);



      koder = new Encoder(ENCODER_DI_A,ENCODER_DI_B,false,Encoder::k4X);



      /* the Watchdog is easy to understand after you understand the rest of the program.

       ** it's hard to explain without that understanding. It's explained at the very

       ** end of the program. Please ignore it for now.

       */

      GetWatchdog().SetExpiration(0.5);

    } // end of RobotDemo()





  void Autonomous(void)

    {



      GetWatchdog().SetEnabled(false); // don't use WD in Autonomous.



      myRobot->Drive(AUTON_DRIVE_SPEED, AUTON_TURN);

      Wait(1.0); // drive 1 second

      myRobot->Drive(0.0, 0.0); // then stop



      // if the arm isn't lowered, lower it:



      if (arm_pot->GetValue() > ARM_POT_LIMIT_LOW)

        { // arm isn't all the way down, lower it



          arm_motor->Set(AUTON_ARM_DOWN_SPEED);



          while ((arm_pot->GetValue() > ARM_POT_LIMIT_LOW) && IsAutonomous())

            ; // loop here over and over 'till arm

          // pot reads <= ARM_POT_LIMIT_DOWN

          // or auton ends, whichever comes first!



          arm_motor->Set(0.0); // then stop arm



        } // arm wasn't all the way down



    } // Auton

    /* The above code avoids the problem of failure of lowering

     ** the arm. The loop will continue as long as the auton

     ** mode is running. When auton mode ends, IsAutonomous()

     ** returns false, and that ends the while() loop

     ** regardless of the reading of the pot.

     */

  void OperatorControl(void)



    {

      int print_counter=0;

      float arm_drive, boom_drive;

      int arm_pot_value;

      int koder_value, koder_value_raw;



      koder->Start();



      // please ignore the Watchdog for now.



      GetWatchdog().SetEnabled(true);



      // keep repeating the Teleop loop for the time (2 minutes so far) of the match:



      while (IsOperatorControl())

        {

          GetWatchdog().Feed();



#if 0



          // this is test code to help explain how the watchdog works. it's only for a demo,

          // it NEVER should go in regular (match) code. The "#if 0" above disables the code.

          // change the 0 to a 1 to enable it.



          while (drive_stick->GetTrigger())

          ; // do nothing really fast!



#endif

          myRobot->ArcadeDrive(drive_stick);



          /*================================== arm winch drive ==========================*/

          // JS Y-value forwards is NEGATIVE, not what you'd expect.

          // JS X value left is NEGATIVE too.

          

          arm_drive = arm_stick->GetY(); // Y deflection raises and lowers arm



 /* if the arm stick trigger is pulled, slow down the

  ** arm motor to give the driver finer control.

  */

          if (arm_stick->GetTrigger())

            {

              arm_drive = arm_drive * ARM_SLOW_RATIO;

            }

          

/* the pot tells us if we've reached the arm's limits

** (the end of its mechanical travel), we have to stop

** the arm from moving if so, otherwise the

** robot or motor will be damaged.

*/

          arm_pot_value = arm_pot->GetValue();



          // if winch is at bottom, keep it from going lower 

          if ((arm_pot_value < ARM_POT_LIMIT_LOW) && (arm_drive < 0))

            {

              arm_drive=0;

            }



          // or at top, stop it from going up

          if ((arm_pot_value > ARM_POT_LIMIT_HIGH)&& (arm_drive > 0))

            {

              arm_drive=0;

            }



          arm_motor->Set(arm_drive); // + pulls arm up





          /* ================================ boom drive ==========================*/



          boom_drive = arm_stick->GetX();



          // we must stop the boom motor at the ends of its

          // mechanical travel, just as we do with the winch:



          // note the "!" below, this means "NOT"



          if ( ((!boom_limit_extend->Get()) && (boom_drive < 0)) // boom extended AND

              // JS says extend?

              || // OR



              ((!boom_limit_retract->Get()) && (boom_drive > 0)) // boom retracted AND                                                                     // JS says retract?

          // JS says retract?

          )



            {

              boom_drive = 0; // if any of these, stop boom motor

            }



          boom_motor->Set(boom_drive);

          

/* If button is pressed on the arm_stick, reset the

 ** encoder to 0. More likely our program would reset

 ** the enoder before it started a drive or turn, 

 ** but this is how you read a joystick button by

 ** the number printed on the button.

 */

          

          if (arm_stick->GetRawButton(ENCODER_RESET_BUTTON))

            

            {

              koder->Reset();

            }



          koder_value = koder->Get();

          koder_value_raw= koder->GetRaw();



          // change the 0 to a 1 below to enable debug printfs. THESE MUST BE DISABLED IN NORMAL

          // OPERATION AND IN COMPETITION!! They slow down execution!



#if 0

          print_counter=print_counter+1;



          if (print_counter > PRINT_COUNT)

            {

              printf("Arm JS y,x; limits, encoders: %f %f %d  %d %d  %d  %d\n",

                  arm_stick->GetY(),

                  arm_stick->GetX(),

                  arm_pot->GetValue(),



                  boom_limit_retract->Get(),

                  boom_limit_extend->Get(),



                  koder_value,

                  koder_value_raw);



              print_counter=0;



            } // time to print??



#endif

        } // while

    } // OperatorConrol

  };



START_ROBOT_CLASS(RobotDemo)

;



#if 0



2010 12 09 Complete version! Drive, arm, and boom work and have limits. BOOM LIMIT GOING

UP DOESN'T WORK WELL WHEN PRINTFS ARE ENABLED!!



Made auton more robust

#endif