GetOverIt/Right1.h
author Michael Tsang
Mon Dec 20 22:50:56 2010 -0800 (2010-12-20)
changeset 65 e2866a39e4fe
parent 63 75840fd7b4b3
child 66 53851d63a011
permissions -rw-r--r--
Fine tuned mechanical parameters
     1 #if 0
     2 /// Copyright (c) Titan Robotics Club. All rights reserved.
     3 ///
     4 /// <module name="Right1.h" />
     5 ///
     6 /// <summary>
     7 ///     This module contains the autonomous routine for starting at the
     8 ///     right corner.
     9 /// </summary>
    10 ///
    11 /// <remarks>
    12 ///     Environment: RobotC for Lego Mindstorms NXT.
    13 /// </remarks>
    14 #endif
    15 
    16 #ifdef MOD_ID
    17     #undef MOD_ID
    18 #endif
    19 #define MOD_ID                  MOD_MAIN
    20 
    21 /**
    22  *  This function implements the autonomous mode with a start position on the
    23  *  right corner.
    24  *
    25  *  @param sm Points to the SM structure.
    26  */
    27 void
    28 AutoRight1(
    29   __inout SM &sm
    30   )
    31 {
    32     float accelValue;
    33 
    34     TFuncName("AutoRight1");
    35     TLevel(TASK);
    36     TEnter();
    37 
    38     switch (sm.currState)
    39     {
    40         case SMSTATE_STARTED:
    41             // Drive forward 2- ft.
    42             PIDDriveSetTarget(g_EncoderDrive,
    43                               20.0, ENCODER_DRIVE_TOLERANCE,
    44                               0.0, ENCODER_TURN_TOLERANCE,
    45                               true);
    46             SMAddWaitEvent(sm, EVTTYPE_PIDDRIVE, -1, -1);
    47             SMWaitEvents(sm, sm.currState + 1, 0, SMF_CLEAR_EVENTS);
    48             break;
    49 
    50         case SMSTATE_STARTED + 1:
    51             // Lower scooper.
    52             ServoSetAngle(g_Scooper, SCOOPER_LOWERED);
    53             TimerSet(g_Timer, 2000);
    54             SMAddWaitEvent(sm, EVTTYPE_TIMER, -1, -1);
    55             SMWaitEvents(sm, sm.currState + 1, 0, SMF_CLEAR_EVENTS);
    56             break;
    57 
    58         case SMSTATE_STARTED + 2:
    59             // Raise scooper to mid level.
    60             ServoSetAngle(g_Scooper, SCOOPER_RAISED);
    61             SMAddWaitEvent(sm, EVTTYPE_SERVO, servoScooper, -1);
    62             SMWaitEvents(sm, sm.currState + 1, 0, SMF_CLEAR_EVENTS);
    63             break;
    64 
    65         case SMSTATE_STARTED + 3:
    66             // Lower arm.
    67             SetArmAngle(ARM_LOWERED);
    68             SMAddWaitEvent(sm, EVTTYPE_SERVO, servoArmLeft, -1);
    69             SMAddWaitEvent(sm, EVTTYPE_SERVO, servoArmRight, -1);
    70             SMWaitEvents(sm, sm.currState + 1, 0, SMF_CLEAR_EVENTS);
    71             break;
    72 
    73         case SMSTATE_STARTED + 4:
    74             // Unload the batons.
    75             motor[motorDispense] = DISPENSE_PUSH;
    76             TimerSet(g_Timer, 2000);
    77             SMAddWaitEvent(sm, EVTTYPE_TIMER, -1, -1);
    78             SMWaitEvents(sm, sm.currState + 1, 0, SMF_CLEAR_EVENTS);
    79             break;
    80 
    81         case SMSTATE_STARTED + 5:
    82             // Stop dispense motor and raise arm.
    83             motor[motorDispense] = 0;
    84             SetArmAngle(ARM_RAISED);
    85             SMAddWaitEvent(sm, EVTTYPE_SERVO, servoArmLeft, -1);
    86             SMAddWaitEvent(sm, EVTTYPE_SERVO, servoArmRight, -1);
    87             SMWaitEvents(sm, sm.currState + 1, 0, SMF_CLEAR_EVENTS);
    88             break;
    89 
    90         case SMSTATE_STARTED + 6:
    91             // Drive backward 4 inches.
    92             PIDDriveSetTarget(g_EncoderDrive,
    93                               -4.0, ENCODER_DRIVE_TOLERANCE,
    94                               0.0, ENCODER_TURN_TOLERANCE,
    95                               true);
    96             SMAddWaitEvent(sm, EVTTYPE_PIDDRIVE, -1, -1);
    97             SMWaitEvents(sm, sm.currState + 1, 0, SMF_CLEAR_EVENTS);
    98             break;
    99 
   100         case SMSTATE_STARTED + 7:
   101             // Turn left 90 degrees.
   102             PIDDriveSetTarget(g_EncoderDrive,
   103                               0.0, ENCODER_DRIVE_TOLERANCE,
   104                               -90.0, ENCODER_TURN_TOLERANCE,
   105                               true);
   106             SMAddWaitEvent(sm, EVTTYPE_PIDDRIVE, -1, -1);
   107             SMWaitEvents(sm, sm.currState + 1, 0, SMF_CLEAR_EVENTS);
   108             break;
   109 
   110         case SMSTATE_STARTED + 8:
   111             // Drive forward 1+ ft.
   112             PIDDriveSetTarget(g_EncoderDrive,
   113                               14.0, ENCODER_DRIVE_TOLERANCE,
   114                               0.0, ENCODER_TURN_TOLERANCE,
   115                               true);
   116             SMAddWaitEvent(sm, EVTTYPE_PIDDRIVE, -1, -1);
   117             SMWaitEvents(sm, sm.currState + 1, 0, SMF_CLEAR_EVENTS);
   118             break;
   119 
   120         case SMSTATE_STARTED + 9:
   121             // Turn right 90-degree.
   122             PIDDriveSetTarget(g_EncoderDrive,
   123                               0.0, ENCODER_DRIVE_TOLERANCE,
   124                               90.0, ENCODER_TURN_TOLERANCE,
   125                               true);
   126             SMAddWaitEvent(sm, EVTTYPE_PIDDRIVE, -1, -1);
   127             SMWaitEvents(sm, sm.currState + 1, 0, SMF_CLEAR_EVENTS);
   128             break;
   129 
   130         case SMSTATE_STARTED + 10:
   131             // Drive forward 1+ ft or until touch active.
   132             PIDCtrlSetPowerLimit(g_PIDCtrl[0], -30, 30);
   133             PIDCtrlSetPowerLimit(g_PIDCtrl[1], -30, 30);
   134             PIDDriveSetTarget(g_EncoderDrive,
   135                               16.0, ENCODER_DRIVE_TOLERANCE,
   136                               0.0, ENCODER_TURN_TOLERANCE,
   137                               true);
   138             SMAddWaitEvent(sm, EVTTYPE_PIDDRIVE, -1, -1);
   139             SMAddWaitEvent(sm, EVTTYPE_TOUCH, -1, -1);
   140             SMWaitEvents(sm, sm.currState + 1, 0, SMF_CLEAR_EVENTS);
   141             break;
   142 
   143         case SMSTATE_STARTED + 11:
   144             // Stop previous drive and drive backward 3 inches.
   145             PIDDriveReset(g_EncoderDrive);
   146             PIDDriveSetTarget(g_EncoderDrive,
   147                               -3.0, ENCODER_DRIVE_TOLERANCE,
   148                               0.0, ENCODER_TURN_TOLERANCE,
   149                               true);
   150             SMAddWaitEvent(sm, EVTTYPE_PIDDRIVE, -1, -1);
   151             SMWaitEvents(sm, sm.currState + 1, 0, SMF_CLEAR_EVENTS);
   152             break;
   153 
   154         case SMSTATE_STARTED + 12:
   155             // Lower scooper.
   156             ServoSetAngle(g_Scooper, SCOOPER_LOWERED);
   157             SMAddWaitEvent(sm, EVTTYPE_SERVO, servoScooper, -1);
   158             SMWaitEvents(sm, sm.currState + 1, 0, SMF_CLEAR_EVENTS);
   159             break;
   160 
   161         case SMSTATE_STARTED + 13:
   162             // Drive forward 3 ft.
   163             PIDDriveSetTarget(g_EncoderDrive,
   164                               38.5, ENCODER_DRIVE_TOLERANCE,
   165                               0.0, ENCODER_TURN_TOLERANCE,
   166                               true);
   167             SMAddWaitEvent(sm, EVTTYPE_PIDDRIVE, -1, -1);
   168             SMWaitEvents(sm, sm.currState + 1, 0, SMF_CLEAR_EVENTS);
   169             break;
   170 
   171         case SMSTATE_STARTED + 14:
   172             TimerSet(g_Timer, 1000);
   173             SMAddWaitEvent(sm, EVTTYPE_TIMER, -1, -1);
   174             SMWaitEvents(sm, sm.currState + 1, 0, SMF_CLEAR_EVENTS);
   175             break;
   176 
   177         case SMSTATE_STARTED + 15:
   178             AccelGetY(g_Accel, accelValue);
   179             if (abs(accelValue) < 1.0)
   180             {
   181                 //
   182                 // We are balanced.
   183                 //
   184                 SMStop(sm);
   185             }
   186             else if (accelValue > 0.0)
   187             {
   188                 //
   189                 // We need to go forward 1 inch.
   190                 //
   191                 PIDDriveSetTarget(g_EncoderDrive,
   192                                   1.0, ENCODER_DRIVE_TOLERANCE,
   193                                   0.0, ENCODER_TURN_TOLERANCE,
   194                                   true);
   195                 SMAddWaitEvent(sm, EVTTYPE_PIDDRIVE, -1, -1);
   196                 SMWaitEvents(sm, SMSTATE_STARTED + 14, 0, SMF_CLEAR_EVENTS);
   197             }
   198             else
   199             {
   200                 //
   201                 // We need to go backward 1 inch.
   202                 //
   203                 PIDDriveSetTarget(g_EncoderDrive,
   204                                   -1.0, ENCODER_DRIVE_TOLERANCE,
   205                                   0.0, ENCODER_TURN_TOLERANCE,
   206                                   true);
   207                 SMAddWaitEvent(sm, EVTTYPE_PIDDRIVE, -1, -1);
   208                 SMWaitEvents(sm, SMSTATE_STARTED + 14, 0, SMF_CLEAR_EVENTS);
   209             }
   210             break;
   211 #if 0
   212         case SMSTATE_STARTED + 14:
   213             PIDCtrlSetPowerLimit(g_PIDCtrl[0], -5, 10);
   214             PIDCtrlSetPowerLimit(g_PIDCtrl[1], -5, 10);
   215             PIDDriveSetTarget(g_BalanceDrive,
   216                               0.0, BALANCE_DRIVE_TOLERANCE,
   217                               0.0, BALANCE_TURN_TOLERANCE,
   218                               false);
   219             sm.currState++;
   220             break;
   221 
   222         case SMSTATE_STARTED + 15:
   223             //
   224             // Loop in this state until autonomous period ends.
   225             //
   226             break;
   227 #endif
   228 
   229         default:
   230             SMStop(sm);
   231             break;
   232     }
   233 
   234     TExit();
   235     return;
   236 }   //AutoRight1