1 / 50

2009 FRC Control System

2009 FRC Control System. C/C++ Dave Doerr, Mentor, Team 67 November 15, 2008. C/C++ Team 67 Beta Test C/C++ Team. John Bottenberg Student, 2 Years FRC, 4 Years FLL Dave Doerr Mentor, 2 Years FRC, 7 Years FLL Paul Doerr Student, 2 Years FRC, 4 Years FLL Rodney Gleason

bob
Download Presentation

2009 FRC Control System

An Image/Link below is provided (as is) to download presentation Download Policy: Content on the Website is provided to you AS IS for your information and personal use and may not be sold / licensed / shared on other websites without getting consent from its author. Content is provided to you AS IS for your information and personal use only. Download presentation by click this link. While downloading, if for some reason you are not able to download a presentation, the publisher may have deleted the file from their server. During download, if you can't get a presentation, the file might be deleted by the publisher.

E N D

Presentation Transcript

  1. 2009 FRC Control System C/C++ Dave Doerr, Mentor, Team 67 November 15, 2008

  2. C/C++Team 67 Beta Test C/C++ Team • John Bottenberg • Student, 2 Years FRC, 4 Years FLL • Dave Doerr • Mentor, 2 Years FRC, 7 Years FLL • Paul Doerr • Student, 2 Years FRC, 4 Years FLL • Rodney Gleason • Mentor, X Years FRC

  3. C/C++Contents The presentation is based on the Beta Test Update 4 release code, 10/22/2008 • WindRiver Workbench • Default Code • HOT Code

  4. C/C++HOT Background In the past, members of the HOT Team have used: • MPLAB • Robolab • NXT-G

  5. C/C++WindRiver Workbench • Eclipse-based Embedded Software Toolset • Project Explorer • Remote Systems Access • Build Console, Console, Terminal • Editors • Debug, Breakpoints, Variables • FIRST Downloader

  6. Software OverviewWindRiver Workbench

  7. C/C++ Robot Operating Modes • Disabled Mode: Before a match begins, robot outputs (motors) are not active. Robots can be programmed to initialize. Driver controls are active. • Autonomous/Hybrid Mode: During the first 15 seconds of a match, driver inputs (joysticks) are not active. Robots can be programmed to use outputs (motors) and sensor inputs (encoders, gyros) for autonomous operation. • Teleoperated Mode: During the remaining 120 seconds of a match, all inputs and outputs are active. Robots are programmed for driver control.

  8. C/C++ C++ Classes • C++ classes are structures that can contain data and functions that can operate on the data. • Class functions are known as member functions or methods.

  9. C/C++ The Robot Classes • The WPI Library provides a choice of two robot base classes, the SimpleRobot and the IterativeRobot. • These classes are intended to be subclassed and extended by teams to use for their own robot code. • The IterativeRobot class is most like the 2008 default robot code. It has: • Disabled, autonomous and teleoperated mode functions that correspond to the 2008 IFI code functions • An external main loop that repetitively executes your code

  10. C/C++ The IterativeRobot Class class IterativeDemo : public IterativeRobot { /* Declarations go here */ // Declare pointer of class RobotDrive RobotDrive *m_RobotDrive; public: // Class Constructor, called once on bootup IterativeDemo(void) { /* Definitions go here */ // Define RobotDrive pointer with PWMs 1,2,3,4 m_RobotDrive = new RobotDrive(1, 3, 2, 4);

  11. C/C++ The IterativeRobot Class // Robot Initialization Function // Called once on bootup // Like 2008 IFI Disabled_Init() void RobotInit(void); // Mode Initialization Functions // Called once on entry to mode void DisabledInit(void); // Like 2008 IFI Autonomous_Init() void AutonomousInit(void); // Like 2008 IFI Teleop_Init() void TeleopInit(void);

  12. C/C++ The IterativeRobot Class // Mode Periodic Functions // Called every 5ms or at 200Hz // Like 2008 IFI Disabled() void DisabledPeriodic(void); // Like 2008 IFI Autonomous() void AutonomousPeriodic(void); // Like 2008 IFI Teleop() void TeleopPeriodic(void) ; };

  13. C/C++ Creating the Iterative Robot Demo • Start WindRiver Workbench • File >> New >> Example… • VxWorks Downloadable Kernel Module >> Next >

  14. C/C++ Creating the Iterative Robot Demo • Iterative Robot Demo Main Program >> Finish

  15. C/C++ Creating the Iterative Robot Demo • Project Explorer >> Expand IterativeDemo • Project Explorer >> Double Click IterativeDemo.cpp

  16. C/C++ Creating the Iterative Robot Demo • Editor >> Maximize IterativeDemo.cpp

  17. C/C++ Exploring the Iterative Robot Demo Extend IterativeRobot class to IterativeDemo and declare objects. classIterativeDemo : public IterativeRobot { // Declare variable for the robot drive system RobotDrive *m_robotDrive;// robot will use PWM 1-4 for drive motors // Declare a variable to use to access the driver station object DriverStation *m_ds;// driver station object UINT32m_priorPacketNumber;// keep track of the most recent DS packet # UINT8m_dsPacketsPerSecond;// keep track of the ds packets // Declare variables for the two joysticks being used Joystick *m_rightStick;// joystick 1 (arcade stick or right tank stick) Joystick *m_leftStick;// joystick 2 (tank left stick) …

  18. C/C++ Exploring the Iterative Robot Demo Define objects in the IterativeDemo Constructor. IterativeDemo(void){ printf("IterativeDemo Constructor Started\n"); // Create a robot using standard right/left robot drive on PWM 1,2,3,4 m_robotDrive = newRobotDrive(1, 3, 2, 4); // Acquire the Driver Station object m_ds = DriverStation::GetInstance(); m_priorPacketNumber = 0; m_dsPacketsPerSecond = 0; // Define joysticks being used at USB ports 1 and 2 on the DS m_rightStick = newJoystick(1); m_leftStick = newJoystick(2); …

  19. C/C++ Exploring the Iterative Robot Demo Initialize loop counters (and anything else that needs initializing…) /********************** Init Routines ***************************/ void RobotInit(void) { // Actions which would be performed only once initialization of the // robot would be put here. } void DisabledInit(void) { m_disabledPeriodicLoops = 0;// Reset the loop counter for disabled mode } void AutonomousInit(void) { m_autoPeriodicLoops = 0;// Reset the loop counter for autonomous mode } void TeleopInit(void) { m_telePeriodicLoops = 0;// Reset the loop counter for teleop mode m_dsPacketsPerSecond = 0;// Reset the number of dsPackets }

  20. C/C++ Exploring the Iterative Robot Demo Run periodic disabled mode code every 5ms or 200Hz. void DisabledPeriodic(void) { // feed the user watchdog at every period when disabled GetWatchdog().Feed(); // increment the number of disabled periodic loops completed m_disabledPeriodicLoops++; // while disabled, printout the number of seconds of disabled so far if ((m_disabledPeriodicLoops % GetLoopsPerSec()) == 0) { printf("Disabled seconds: %d\r\n", (m_disabledPeriodicLoops / GetLoopsPerSec())); } }

  21. C/C++ Exploring the Iterative Robot Demo Run periodic autonomous mode code every 5ms or 200Hz. void AutonomousPeriodic(void) { // feed the user watchdog at every period // The watchdog will shut down the robot if not fed GetWatchdog().Feed(); m_autoPeriodicLoops++; if (m_autoPeriodicLoops == 1) { m_robotDrive->Drive(0.5, 0.0);// drive forwards at half speed } if (m_autoPeriodicLoops == (2 * GetLoopsPerSec())) { // After 2 seconds, stop the robot m_robotDrive->Drive(0.0, 0.0);// stop robot } }

  22. C/C++ Exploring the Iterative Robot Demo Run periodic teleoperated mode code every 5ms or 200Hz. void TeleopPeriodic(void) { // increment the number of teleop periodic loops completed m_telePeriodicLoops++; // Code placed here is called every loop // put 200Hz Jaguar control here if ((m_telePeriodicLoops % 2) == 0) { // Code placed here is called every other loop // put 100Hz Victor control here } if ((m_telePeriodicLoops % 4) == 0) { // put 50Hz Victor control here }

  23. C/C++ Exploring the Iterative Robot Demo Periodic teleoperated mode (continued) if (m_ds->GetPacketNumber() != m_priorPacketNumber) { /* Code placed here will be called only when a new packet of information * has been received by the Driver Station. */ m_priorPacketNumber = m_ds->GetPacketNumber(); m_dsPacketsPerSecond++;// increment DS packets received // put Driver Station-dependent code here // Demonstrate the use of the Joystick buttons DemonstrateJoystickButtons(m_rightStick, m_rightStickButtonState, "Right Stick", &m_solenoids[1]); DemonstrateJoystickButtons(m_leftStick, m_leftStickButtonState, "Left Stick ", &m_solenoids[5]);

  24. C/C++ Exploring the Iterative Robot Demo Periodic teleoperated mode (continued) // Tank or Arcade Drive? if (m_rightStick->GetZ() > 0) { // Arcade Drive m_robotDrive->ArcadeDrive(m_rightStick); // if newly entered arcade drive, print out a message if (m_driveMode != ARCADE_DRIVE) { printf("Arcade Drive\n"); m_driveMode = ARCADE_DRIVE; }

  25. C/C++ Exploring the Iterative Robot Demo Periodic teleoperated mode (continued) } else { // Tank Drive m_robotDrive->TankDrive(m_leftStick, m_rightStick); if (m_driveMode != TANK_DRIVE) { // if newly entered tank drive, print out a message printf("Tank Drive\n"); m_driveMode = TANK_DRIVE; } } } // END if (m_ds->GetPacketNumber()... } // TeleopPeriodic(void)

  26. C/C++ Build and Run the Iterative Robot Demo To build the project (compile and link): • (Right Click Project) >> Build Project Temporary Deployment: Load the demo into RAM and run: • (Right Click Project) >> Run Kernel Task… • Run • Permanent Deployment: Load the demo into flash • FIRST >> Download

  27. C/C++ Exploring the HOT_Bot Variable Delarations class HOT_Bot : public IterativeRobot { // Declare RobotDrive pointer RobotDrive *m_myRobot; // Declare pointers of class GamePad GamePad *m_gamePad1;// gamepad 1 (drive control) GamePad *m_gamePad2;// gamepad 2 (arm control) // Declare pointers of class Victor Victor *m_handTopMotor;// hand top motor Victor *m_handBottomMotor;// hand bottom motor Victor *m_armMotor;// arm motor

  28. C/C++ Exploring the HOT_Bot Variable Definitions // The HOT_Bot Constructor HOT_Bot(void){ // Define robot drive on PWMs 1,2,3,4 m_myRobot = new RobotDrive(1, 3, 2, 4); // Define gamepads on USB ports 1 & 2 on DS m_gamePad1 = new GamePad(1); m_gamePad2 = new GamePad(2); // Define hand and arm motors on PWMs 5,6,7 m_handTopMotor = new Victor(5); m_handBottomMotor = new Victor(6); m_armMotor = new Victor(7);

  29. C/C++ Exploring the HOT_Bot // Tank Drive using GamePad 1 // Left and Right joystick y-axes float leftY = m_gamePad1->GetLeftY(); float rightY = m_gamePad1->GetRightY(); // Drive with RobotDrive TankDrive method m_myRobot->TankDrive(leftY, rightY);

  30. C/C++ Exploring the HOT_Bot

  31. C/C++ Exploring the HOT_Bot // PID control of arm if (m_gamePad2->GetButton01()) { // Get the SetPoint float sP = ARM_POSITION_GROUND; // Get the ProcessVariable float pV = m_armPot->GetVoltage(); // Get the ManipulatedVariable float mV = m_armPID->CalculateMV(sP,pV); // Set the Arm Motor Speed m_armMotor->Set(mV); }

  32. C/C++ Exploring the HOT_Bot // PID control of arm if (m_gamePad2->GetButton01()){ } elseif (m_gamePad2->GetButton02()){ } elseif (m_gamePad2->GetButton03()){ } elseif (m_gamePad2->GetButton04()){ } // manual control of arm: left joystick else { m_armMotor->Set(m_gamePad2->GetLeftY()); }

  33. C/C++ Exploring the HOT_Bot

  34. C/C++ Exploring the HOT_Bot Initializing Autonomous Mode void AutonomousInit(void) { m_autoPeriodicLoops = 0;// reset loop counter m_autoState = 0;// reset mode state m_armPID->Reset();// reset arm PID control m_gyro->Reset();// reset gyro m_leftEncoder->Start();// Start the encoder counts m_rightEncoder->Start(); m_bearing = 0.0; }

  35. C/C++ Exploring the HOT_Bot Autonomous Program: Sensor Display void AutonomousProgram_00(void) { float angle = m_gyro->GetAngle(); int leftCount = m_leftEncoder->Get(); int rightCount = m_rightEncoder->Get(); if( (m_autoPeriodicLoops % 20) == 0 ) { printf("gyro = %f, encoder = %d %d\n", angle, leftCount, rightCount); } }

  36. C/C++ Exploring the HOT_Bot Autonomous Program: Drive using Gyro void AutonomousProgram_02(void) { float speed = 0.40; // gyro measures positive counterclockwise float heading = -m_gyro->GetAngle(); AutoDrive01( speed, heading ); }

  37. C/C++ Exploring the HOT_Bot Autonomous Drive Program: void AutoDrive01(float speed, float angle) { float speedL = speed; float speedR = speed; angle = angle*5.; if (angle > 0.0) { if (angle > +90.0) angle = +90.0; speedR = speed*(1.0 - angle/90.0); } elseif (angle < 0.0) { if (angle < -90.0) angle = -90.0; speedL = speed*(1.0 + angle/90.0); } m_myRobot->TankDrive(speedL, speedR); }

  38. C/C++ Exploring the HOT_Bot Autonomous Program: Drive using Encoders State Machine: • State 0: Start driving • State 1: Measure and test • State 2: Stop

  39. C/C++ Exploring the HOT_Bot Autonomous Program: Drive using Encoders void AutonomousProgram_03(void) { float speed; float heading; switch(m_autoState) { case 0:// execute once to start driving robot speed = 0.40; heading = 0.0; AutoDrive01( speed, heading ); m_autoState = 1; break;

  40. C/C++ Exploring the HOT_Bot Autonomous Program: Drive using Encoders (continued) case 1: // how far have we gone? m_leftEncoderCount = m_leftEncoder->Get(); m_rightEncoderCount = m_rightEncoder->Get(); float distance = MM_PER_COUNT*(m_leftEncoderCount + m_rightEncoderCount)/2.0; // stop when we get to the target distance if( distance > 6000. ) { AutoDrive01( 0.0, 0.0); m_autoState = 2; } break;

  41. C/C++ Exploring the HOT_Bot Autonomous Program: Zig Zag State Machine: • State 0: Set bearing to 30 degrees • State 1: Zig > Drive 2000mm at +30 degrees • State 2: Zag > Drive 4000mm at -30 degrees • State 3: Zig > Drive 2000mm at + 30 degrees • State 4: Stop

  42. C/C++ Exploring the HOT_Bot Autonomous Program: Zig Zag void AutonomousProgram_04(void) { float heading; float speed; float distance; switch(m_autoState) { case 0:// set initial bearing m_bearing = 30.0; m_autoState = 1; break;

  43. C/C++ Exploring the HOT_Bot Autonomous Program: Zig Zag (continued) Zig case 1:// try to drive straight heading = -m_gyro->GetAngle(); speed = 0.40; AutoDrive01( speed, heading - m_bearing ); // how far have we gone? distance = MM_PER_COUNT*(m_leftEncoder->Get() + m_rightEncoder->Get())/2.0;

  44. C/C++ Exploring the HOT_Bot Autonomous Program: Zig Zag (continued) Zig /* case 1 continued */ // move on to next state after reaching target if( distance > 2000. ) { m_leftEncoder->Reset();// Reset the encoder counts m_rightEncoder->Reset(); m_bearing = -30.0;// set the new bearing m_autoState = 2; } break;

  45. C/C++ Exploring the HOT_Bot Autonomous Program: Zig Zag (continued) Zaag case 2:// try to drive straight heading = -m_gyro->GetAngle(); speed = 0.40; AutoDrive01( speed, heading - m_bearing ); // how far have we gone? distance = MM_PER_COUNT*(m_leftEncoder->Get() + m_rightEncoder->Get())/2.0;

  46. C/C++ Exploring the HOT_Bot Autonomous Program: Zig Zag (continued) Zaag /* case 2 continued */ // move on to next state after reaching target if( distance > 4000. ) { m_leftEncoder->Reset();// Reset the encoder counts m_rightEncoder->Reset(); m_bearing = 30.0;// set the new bearing m_autoState = 2; } break;

  47. C/C++ Exploring the HOT_Bot Autonomous Program: Zig Zag (continued) Zig case 3:// try to drive straight heading = -m_gyro->GetAngle(); speed = 0.40; AutoDrive01( speed, heading - m_bearing ); // how far have we gone? distance = MM_PER_COUNT*(m_leftEncoder->Get() + m_rightEncoder->Get())/2.0;

  48. C/C++ Exploring the HOT_Bot Autonomous Program: Zig Zag (continued) Zig /* case 3 continued */ // stop after reaching target if( distance > 2000. ) { AutoDrive01( 0.0, 0.0); m_autoState = 4; } break;

  49. C/C++ 2008 Autonomous at IRI

  50. C/C++ ?

More Related