CascadeEffect/ParkGoal6541.h
author TrcTeam1
Thu Feb 05 14:31:04 2015 -0800 (2015-02-05)
changeset 68 c3e3e943a1b6
parent 66 01d92df81d01
permissions -rw-r--r--
Added motor tester
     1 #if 0
     2 /// Copyright (c) Titan Robotics Club. All rights reserved.
     3 ///
     4 /// <module name="ParkGoal.h" />
     5 ///
     6 /// <summary>
     7 ///     This module contains the implementation of the Park Goal strategy.
     8 /// </summary>
     9 ///
    10 /// <remarks>
    11 ///     Environment: RobotC for Lego Mindstorms NXT.
    12 /// </remarks>
    13 #endif
    14 
    15 /**
    16  *  This function drives the robot down the ramp and pulls the goal into
    17  *  the parking zone autonomously.
    18  *
    19  *  @param sm Specifies the state machine.
    20  *  @param delay Specifies the delay if any.
    21  *  @param parkOppoDefense Specifies whether to anticipate opponent defense.
    22  *  @param parkFarEnd Specifies whether to go back to the far end of the field.
    23  */
    24 void DoParkGoal(SM &sm, unsigned long delay, int parkOppoDefense, int parkFarEnd)
    25 {
    26     static float startHeading = 0.0;
    27     static float startPos = 0.0;
    28 //    PIDCtrlDebugInfo(g_elevatorPidCtrl);
    29 //    PIDCtrlDisplayInfo(1, g_encoderDrivePidCtrl);
    30 //    nxtDisplayTextLine(3, "lIR=%4.1f,rIR=%4.1f",
    31 //                       IRSeekerGetACDir(g_leftIR),
    32 //                       IRSeekerGetACDir(g_rightIR));
    33 //    PIDCtrlDisplayInfo(3, g_gyroTurnPidCtrl);
    34     if (SMIsReady(sm))
    35     {
    36         int currState = SMGetState(sm);
    37         float angle;
    38         float distTravel;
    39 
    40         switch (currState)
    41         {
    42             case SMSTATE_STARTED:
    43                 //
    44                 // Raise the elevator.
    45                 //
    46                 startHeading = PIDCtrlModeGetInput(g_gyroTurnPidCtrl);
    47                 PIDMotorSetTarget(g_elevatorMotor,
    48                                   ELEVATOR_MID_GOAL,
    49                                   false,
    50                                   &sm,
    51                                   EVTTYPE_PIDELEVATOR);
    52                 ServoSetAngle(g_goalCaptureServo, GOALCAPTURE_UP);
    53                 if (delay != 0)
    54                 {
    55                     TimerSet(g_timer, delay, &sm, EVTTYPE_TIMER);
    56                     SMAddWaitEvent(sm, EVTTYPE_TIMER);
    57                     SMWaitEvents(sm, currState + 1);
    58                 }
    59                 else
    60                 {
    61                     SMSetState(sm, currState + 1);
    62                 }
    63                 break;
    64 
    65             case SMSTATE_STARTED + 1:
    66                 //
    67                 // Drive backward down the ramp (first segment).
    68                 //
    69                 PIDCtrlSetPowerLimits(g_encoderDrivePidCtrl, -20, 20);
    70                 PIDDriveSetTarget(g_encoderPidDrive,
    71                                   -65.0,
    72                                   0.0,
    73                                   false,
    74                                   &sm,
    75                                   EVTTYPE_PIDDRIVE,
    76                                   3000);
    77                 SMAddWaitEvent(sm, EVTTYPE_PIDDRIVE);
    78                 SMWaitEvents(sm, currState + 1);
    79                 break;
    80 
    81             case SMSTATE_STARTED + 2:
    82                 //
    83                 // Drive slowly towards the goal (second segment).
    84                 //
    85                 PIDCtrlSetPowerLimits(g_encoderDrivePidCtrl, -15, 0);
    86                 PIDDriveSetTarget(g_encoderPidDrive,
    87                                   -24.0,
    88                                   0.0,
    89                                   false,
    90                                   &sm,
    91                                   EVTTYPE_PIDDRIVE,
    92                                   3000);
    93                 SMAddWaitEvent(sm, EVTTYPE_PIDDRIVE);
    94                 SMWaitEvents(sm, currState + 1);
    95                 break;
    96 
    97             case SMSTATE_STARTED + 3:
    98                 //
    99                 // Clamp down on mid-goal.
   100                 //
   101                 ServoSetAngle(g_goalCaptureServo,GOALCAPTURE_DOWN);
   102                 TimerSet(g_timer, 300, &sm, EVTTYPE_TIMER);
   103                 SMAddWaitEvent(sm, EVTTYPE_TIMER);
   104                 SMWaitEvents(sm, currState + 1);
   105                 break;
   106 
   107             case SMSTATE_STARTED + 4:
   108                 //
   109                 // Turn towards the parking zone for mid-goal.
   110                 //
   111                 PIDCtrlSetPowerLimits(g_encoderDrivePidCtrl,
   112                                       DRIVE_MIN_VALUE, DRIVE_MAX_VALUE);
   113                 angle = 35.0 -
   114                         (PIDCtrlModeGetInput(g_gyroTurnPidCtrl) -
   115                          startHeading);
   116                 PIDDriveSetTarget(g_encoderPidDrive,
   117                                   0.0,
   118                                   angle,
   119                                   false,
   120                                   &sm,
   121                                   EVTTYPE_PIDDRIVE,
   122                                   2000);
   123                 SMAddWaitEvent(sm, EVTTYPE_PIDDRIVE);
   124                 SMWaitEvents(sm, currState + 1);
   125                 break;
   126 
   127             case SMSTATE_STARTED + 5:
   128                 //
   129                 // Drive forward to the parking zone for mid-goal.
   130                 //
   131                 startPos = DriveGetYPos(g_drive);
   132                 PIDCtrlSetPowerLimits(g_encoderDrivePidCtrl, -40, 40);
   133                 PIDDriveSetTarget(g_encoderPidDrive,
   134                                   70.0,
   135                                   0.0,
   136                                   false,
   137                                   &sm,
   138                                   EVTTYPE_PIDDRIVE,
   139                                   3000);
   140                 SMAddWaitEvent(sm, EVTTYPE_PIDDRIVE);
   141                 SMWaitEvents(sm, currState + 1);
   142                 break;
   143 
   144             case SMSTATE_STARTED + 6:
   145                 //
   146                 // Turn around to push mid-goal in.
   147                 //
   148                 angle = 190.0 -
   149                         (PIDCtrlModeGetInput(g_gyroTurnPidCtrl) -
   150                          startHeading);
   151                 ServoSetAngle(g_drawBridgeServo, DRAWBRIDGE_RELEASE);
   152                 PIDDriveSetTarget(g_encoderPidDrive,
   153                                   0.0,
   154                                   angle,
   155                                   false,
   156                                   &sm,
   157                                   EVTTYPE_PIDDRIVE,
   158                                   3000);
   159                 SMAddWaitEvent(sm, EVTTYPE_PIDDRIVE);
   160                 SMWaitEvents(sm, currState + 1);
   161                 break;
   162 
   163             case SMSTATE_STARTED + 7:
   164                 //
   165                 // Drive towards the parking zone with mid-goal.
   166                 //
   167                 distTravel = DriveGetYPos(g_drive) - startPos;
   168                 PIDCtrlSetPowerLimits(g_encoderDrivePidCtrl, -25, 25);
   169                 PIDDriveSetTarget(g_encoderPidDrive,
   170                                   -(20.0 - distTravel),
   171                                   0.0,
   172                                   false,
   173                                   &sm,
   174                                   EVTTYPE_PIDDRIVE,
   175                                   2000);
   176                 SMAddWaitEvent(sm, EVTTYPE_PIDDRIVE);
   177                 SMWaitEvents(sm, currState + 1);
   178                 break;
   179 
   180             case SMSTATE_STARTED + 8:
   181                 //
   182                 // Mid-Goal release
   183                 //
   184                 PIDCtrlSetPowerLimits(g_encoderDrivePidCtrl,
   185                                       DRIVE_MIN_VALUE, DRIVE_MAX_VALUE);
   186                 ServoSetAngle(g_drawBridgeServo, DRAWBRIDGE_CAPTURE);
   187                 ServoSetAngle(g_goalCaptureServo, GOALCAPTURE_UP);
   188                 TimerSet(g_timer, 200, &sm, EVTTYPE_TIMER);
   189                 SMAddWaitEvent(sm, EVTTYPE_TIMER);
   190                 SMWaitEvents(sm, currState + 1);
   191                 break;
   192 
   193             case SMSTATE_STARTED + 9:
   194                 //
   195                 // Bring elevator to tall goal or down.
   196                 //
   197                 if (parkFarEnd == PARKFAREND_GRAB_TALLGOAL)
   198                 {
   199                     PIDMotorSetTarget(g_elevatorMotor,
   200                                       ELEVATOR_HIGH_GOAL,
   201                                       false);
   202                 }
   203                 else
   204                 {
   205                     PIDMotorSetTarget(g_elevatorMotor,
   206                                       ELEVATOR_DOWN_POS,
   207                                       false);
   208                 }
   209                 PIDDriveSetTarget(g_encoderPidDrive,
   210                                   12.0,
   211                                   0.0,
   212                                   false,
   213                                   &sm,
   214                                   EVTTYPE_PIDDRIVE,
   215                                   1000);
   216                 SMAddWaitEvent(sm, EVTTYPE_PIDDRIVE);
   217                 SMWaitEvents(sm, currState + 1);
   218                 break;
   219 
   220             case SMSTATE_STARTED + 10:
   221                 //
   222                 // Turn toward tall goal.
   223                 //
   224                 angle = 34.0 -      //36.0
   225                         (PIDCtrlModeGetInput(g_gyroTurnPidCtrl) -
   226                          startHeading);
   227                 PIDDriveSetTarget(g_encoderPidDrive,
   228                                   0.0,
   229                                   angle,
   230                                   false,
   231                                   &sm,
   232                                   EVTTYPE_PIDDRIVE,
   233                                   3000);
   234                 SMAddWaitEvent(sm, EVTTYPE_PIDDRIVE);
   235                 if (parkFarEnd == PARKFAREND_YES ||
   236                     parkFarEnd == PARKFAREND_GRAB_TALLGOAL)
   237                 {
   238                     SMWaitEvents(sm, currState + 1);
   239                 }
   240                 else
   241                 {
   242                     SMWaitEvents(sm, 1000);
   243                 }
   244                 break;
   245 
   246             case SMSTATE_STARTED + 11:
   247                 //
   248                 // Drive toward tall goal.
   249                 //
   250                 PIDDriveSetTarget(g_encoderPidDrive,
   251                                   0.0,
   252                                   -80.0,
   253                                   0.0,
   254                                   false,
   255                                   &sm,
   256                                   EVTTYPE_PIDDRIVE,
   257                                   3000);
   258                 SMAddWaitEvent(sm, EVTTYPE_PIDDRIVE);
   259                 if (parkFarEnd == PARKFAREND_GRAB_TALLGOAL)
   260                 {
   261                     SMWaitEvents(sm, currState + 1);
   262                 }
   263                 else
   264                 {
   265                     SMWaitEvents(sm, 1000);
   266                 }
   267                 break;
   268 
   269             case SMSTATE_STARTED + 12:
   270                 //
   271                 // Drive slowly towards tall goal.
   272                 //
   273                 PIDCtrlSetPowerLimits(g_encoderDrivePidCtrl, -30, 0);
   274                 PIDDriveSetTarget(g_encoderPidDrive,
   275                                   -16.0, //-16.0 -18.0
   276                                   0.0,
   277                                   false,
   278                                   &sm,
   279                                   EVTTYPE_PIDDRIVE,
   280                                   1500);
   281                 SMAddWaitEvent(sm, EVTTYPE_PIDDRIVE);
   282                 SMWaitEvents(sm, currState + 1);
   283                 break;
   284 
   285             case SMSTATE_STARTED + 13:
   286                 //
   287                 // Clamp down on tall goal.
   288                 //
   289                 ServoSetAngle(g_goalCaptureServo,GOALCAPTURE_DOWN);
   290                 TimerSet(g_timer, 200, &sm, EVTTYPE_TIMER);
   291                 SMAddWaitEvent(sm, EVTTYPE_TIMER);
   292                 SMWaitEvents(sm, currState + 1);
   293                 break;
   294 
   295             case SMSTATE_STARTED + 14:
   296                 //
   297                 // Drive forward to the parking zone with tall goal.
   298                 //
   299                 ServoSetAngle(g_drawBridgeServo, DRAWBRIDGE_RELEASE);
   300                 startPos = DriveGetYPos(g_drive);
   301                 PIDCtrlSetPowerLimits(g_encoderDrivePidCtrl, MOTOR_MIN_VALUE, MOTOR_MAX_VALUE);
   302                 PIDDriveSetTarget(g_encoderPidDrive,
   303                                   115.0, //115.0, 112.0
   304                                   0.0,
   305                                   false,
   306                                   &sm,
   307                                   EVTTYPE_PIDDRIVE,
   308                                   6000);
   309                 SMAddWaitEvent(sm, EVTTYPE_PIDDRIVE);
   310                 SMWaitEvents(sm, currState + 1);
   311                 break;
   312 
   313             case SMSTATE_STARTED + 15:
   314                 //
   315                 // Turn around with tall goal.
   316                 //
   317                 angle = 180.0 -
   318                         (PIDCtrlModeGetInput(g_gyroTurnPidCtrl) -
   319                          startHeading);
   320                 ServoSetAngle(g_drawBridgeServo, DRAWBRIDGE_RELEASE);
   321                 PIDDriveSetTarget(g_encoderPidDrive,
   322                                   0.0,
   323                                   angle,
   324                                   false,
   325                                   &sm,
   326                                   EVTTYPE_PIDDRIVE,
   327                                   3000);
   328                 SMAddWaitEvent(sm, EVTTYPE_PIDDRIVE);
   329                 SMWaitEvents(sm, currState + 1);
   330                 break;
   331 
   332             case SMSTATE_STARTED + 16:
   333                 //
   334                 // Bring the elevator back down.
   335                 //
   336                 ServoSetAngle(g_drawBridgeServo, DRAWBRIDGE_CAPTURE);
   337                 PIDMotorSetTarget(g_elevatorMotor,
   338                                   ELEVATOR_DOWN_POS,
   339                                   false,
   340                                   &sm,
   341                                   EVTTYPE_PIDELEVATOR);
   342                 SMAddWaitEvent(sm, EVTTYPE_PIDELEVATOR);
   343                 SMWaitEvents(sm, currState + 1);
   344                 break;
   345 
   346             default:
   347                 SMStop(sm);
   348                 PlayTone(440, 50);
   349                 break;
   350         }
   351     }
   352 }   //DoParkGoal