PID Control

BTB1337 · Tue Feb 24, 2015 4:01 pm


	MHTS wrote: There are two issues. When you said +/- 20 degrees, are they from the reading of E on the NXT display? Or are they the degrees error from your physical observation? If it's the E value from the NXT display, PID is supposed to make error down to within tolerance (whatever it was). If your tolerance was 1 degree, then E should read 1 or less at the end. If it doesn't, then you need to tune Kp to the correct value until E ended up within tolerance. If it is your physical observation that the robot is 20 degree from where you want it to be, then you need to tune the DEGREES_PER_CLICK until the robot physically turn 90 degree if you set the target to 90. Am I understanding your issue correctly? If by changing to a fresh battery that the robot turns correctly, that's just the way PID behaves. I think one of my previous replies have talked about this subject. You need to change to a fresh battery for competition. If the battery is too weak, then the robot can't reach target. You can fix this somewhat by increasing Kp to a larger number. +/- 20 degree seems to be a lot. Usually, a weak battery will leave the error around a few degrees. In any case, play around with Kp to see if it improves things.

Ok I got the encoders to work pretty well and we finally got our gyro delivered on saturday, (the day we needed it by), what good that did for us haha

As for our ir program i have 2 programs, one that had all the PID info and stuff for each 3 positions (i just put a bunch of blocks of Pid ctrl set target) and commented out each position if that make sense, like i would have the state machine and do state 1 forward state 2 left state 3 kickstand then comment it out and put position 1, then i put state 1 forward state 2 right state 3 kickstand and comment it out, if that makes sense. i'll show the code in a minute.

Then I have another with an if statement ( and i know that i did the while improperly but i'm not quite sure the proper way to break the loop, would it be break; or return;? i don't know) it was basically a while loop with 3 if statements that said set irservo = 130 then if irseeker ==6 then run position 1, else set irserver = 145 (or whatever the number is) if irseeker = 6 then run position 2, etc for all 3 positions.

The way our irseeker works is that we mounted it on a servo and used a debug program to adjust the servo to be perfectly in the middle of zone 6 on the IR if the center goal was in position 1 position 2 and position 3 rather than having the robot move or using 3 separate numbers.

So basically i attempted to combine the irSeeker program with the PID program with state machine and I got it past the ir where it cycled through the positions and if it found zone 6 it would go to the specific if statement, the problem was getting it out of the if statement and moving to the correct state. hopefully when you read the code you can try to figure out how it works because i'm awful at explaining stuff and i appologize for this.

irSeeker.c combined with pid enc...the commented out while loop at the bottom is the basic idea of the origional irSeeker.c


	Code: #pragma config(Hubs, S1, HTMotor, HTMotor, HTServo, none) #pragma config(Sensor, S1, , sensorI2CMuxController) #pragma config(Sensor, S2, irSeeker, sensorI2CCustom) #pragma config(Sensor, S3, htComp, sensorI2CHiTechnicCompass) #pragma config(Sensor, S4, htEOPD, sensorI2CCustom) #pragma config(Motor, mtr_S1_C1_1, rightWheel, tmotorTetrix, openLoop, encoder) #pragma config(Motor, mtr_S1_C1_2, leftWheel, tmotorTetrix, openLoop, reversed, encoder) #pragma config(Motor, mtr_S1_C2_1, rightLifter, tmotorTetrix, PIDControl, reversed) #pragma config(Motor, mtr_S1_C2_2, leftLifter, tmotorTetrix, PIDControl) #pragma config(Servo, srvo_S1_C3_1, servo1, tServoNone) #pragma config(Servo, srvo_S1_C3_2, servo2, tServoNone) #pragma config(Servo, srvo_S1_C3_3, rightDoor, tServoStandard) #pragma config(Servo, srvo_S1_C3_4, leftDoor, tServoStandard) #pragma config(Servo, srvo_S1_C3_5, goalHook, tServoStandard) #pragma config(Servo, srvo_S1_C3_6, irServo, tServoStandard) //!!Code automatically generated by 'ROBOTC' configuration wizard !!// // include files #include "ftclib/trcdefs.h" #include "ftclib/dbgtrace.h" #include "ftclib/sm.h" #include "ftclib/drive.h" #include "ftclib/pidctrl.h" #include "ftclib/piddrive.h" #include "RobotCDrivers/Drivers/hitechnic-irseeker-v2.h" #include "JoystickDriver.c" // define distance with encoders #define ENC_KP 1.7 // change this to meet distance target #define ENC_KI 0.0015 #define ENC_KD 0.5 #define ENC_TOLERANCE 1.0 #define ENC_SETTLING 200 // define turns with encoder #define TURN_KP 1.5 // change this to meet turn target #define TURN_KI 0.0 #define TURN_KD 0.5 #define TURN_TOLERANCE 1.8 #define TURN_SETTLING 200 #define CLICKS_PER_INCH 118.5 // change this to adjust actual distance #define CLICKS_PER_DEGREE 17.0 // change this to adjust actual degree #define EVTTYPE_PIDDRIVE (EVTTYPE_NONE + 1) DRIVE g_drive; //create the drive object. PIDCTRL g_yPidCtrl; //create the Y PID controller PIDCTRL g_turnPidCtrl; //create the turn PID controller PIDDRIVE g_pidDrive; //create the PID drive object SM g_autoSM; //create the autonomous state machine object // calculate all the math float PIDCtrlGetInput(PIDCTRL &pidCtrl) { float inputValue = 0.0; if (IsSameReference(pidCtrl, g_yPidCtrl)) { inputValue = (nMotorEncoder[leftWheel] + nMotorEncoder[rightWheel])/2.0/CLICKS_PER_INCH; } else if (IsSameReference(pidCtrl, g_turnPidCtrl)) { inputValue = (nMotorEncoder[leftWheel] - nMotorEncoder[rightWheel])/2.0/CLICKS_PER_DEGREE; } return inputValue; } // declaring variables int on; void initializeRobot() { servoTarget[rightDoor] = 0; servoTarget[leftDoor] = 180; servoTarget[irServo] = 130; return; } int timeLeft; int runTime; void runMode() { // reset Timer1 clearTimer(T1); eraseDisplay(); while(true) { // Calculate runTime // Calculate the Time left to engage test mode runTime = time1[T1].001; timeLeft = abs(runTime-10); // display instuctions displayTextLine(1, " Press Enter"); displayTextLine(3, " to Engage"); displayTextLine(5, " Test Mode"); displayTextLine(7, " Time Left: %d", timeLeft); // if statement to switch waitForStart() if(timeLeft <= 10 && nNxtButtonPressed == 3) { eraseDisplay(); displayTextLine(2, " Test"); displayTextLine(4, " Mode"); sleep(5000); return; } else if(timeLeft == 0) { eraseDisplay(); displayTextLine(2, " Competition"); displayTextLine(4, " Mode"); sleep(3000); waitForStart(); return; } } } task main() { initializeRobot(); waitForStart(); //runMode(); // ac directional value. int _dirAC; // we are going to set DSP mode to 1200 Hz. By the way, DSP_1200 is a constant = 0, DSP_600 = 1. /tHTIRS2DSPMode _mode = DSP_1200; // attempt to set to DSP mode. if (HTIRS2setDSPMode(irSeeker, _mode) == 0) { // unsuccessful at setting the mode. Make a noise to get their attention. PlaySound(soundBeepBeep); return; }/ // read the current modulated signal direction _dirAC = HTIRS2readACDir(irSeeker); // Now you can write your own code using _dirAC. Also, you should only need to set the mode once, so // from now on, you just need to use the above line to poll/display the signal direction, perhaps in an endless while loop. // Initialize the drive object. DriveInit(g_drive, leftWheel, rightWheel); // Initialize the yPidCtrl object. PIDCtrlInit(g_yPidCtrl, ENC_KP, ENC_KI, ENC_KD, ENC_TOLERANCE, ENC_SETTLING); // Initialize the turnPidCtrl object. PIDCtrlInit(g_turnPidCtrl, TURN_KP, TURN_KI, TURN_KD, TURN_TOLERANCE, TURN_SETTLING); // Initialize the pidDrive object. PIDDriveInit(g_pidDrive, g_drive, g_yPidCtrl, g_turnPidCtrl); // Initialize the autonomous state machine and start it. SMInit(g_autoSM); SMStart(g_autoSM); // error: PgmCnt: 0001BB // Type: 34 while (true) { // display debug info on nxt displayTextLine(0, "state=%d", SMGetState(g_autoSM)); PIDCtrlDisplayInfo(1, g_yPidCtrl); PIDCtrlDisplayInfo(3, g_turnPidCtrl); // Check if state machine is ready. if (SMIsReady(g_autoSM)) { int currState = SMGetState(g_autoSM); switch (currState) { case SMSTATE_STARTED: on = 1; while (true) { servoTarget[irServo] = 130; wait10Msec(200); if (HTIRS2readACDir(irSeeker) == 6) { // position 1 SMSetState(g_autoSM, SMSTATE_STARTED + 1); break; } // if irSeeker = 6 else // move irServo to 140 { servoTarget[irServo] = 140; } wait10Msec(200); if (_dirAC = HTIRS2readACDir(irSeeker) == 6) { //position 2 SMWaitEvents(g_autoSM, SMSTATE_STARTED + 7; break; } // if left ir seeker = 5 and right ir seeker = 2 else { servoTarget[irServo] = 157; } wait10Msec(200); if (_dirAC = HTIRS2readACDir(irSeeker) == 6) { //position 3 SMWaitEvents(g_autoSM, SMSTATE_STARTED + 12; break; } } SMWaitEvents(g_autoSM, currState); break; // POSITION 1 case SMSTATE_STARTED + 1: // Drive forward for 24 inches. PIDDriveSetTarget(g_pidDrive, 24.0, 0.0, false, &g_autoSM, EVTTYPE_PIDDRIVE, 2500); // Tell state machine to wait until done. SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE); // Tell state machine to go to next state when done. SMWaitEvents(g_autoSM, currState + 1); break; case SMSTATE_STARTED + 2: // Turn left 75 degrees. PIDDriveSetTarget(g_pidDrive, 0.0, -75.0, false, &g_autoSM, EVTTYPE_PIDDRIVE, 2500); // Tell state machine to wait until done. SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE); // Tell state machine to go to next state when done. SMWaitEvents(g_autoSM, currState + 1); break; case SMSTATE_STARTED + 3: // Drive forward 30 inches. PIDDriveSetTarget(g_pidDrive, 30.0, 0.0, false, &g_autoSM, EVTTYPE_PIDDRIVE, 2500); // Tell state machine to wait until done. SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE); // Tell state machine to go to next state when done. SMWaitEvents(g_autoSM, currState + 1); break; case SMSTATE_STARTED + 4: // Turn left 55 degrees. PIDDriveSetTarget(g_pidDrive, 0.0, -55.0, false, &g_autoSM, EVTTYPE_PIDDRIVE, 2500); // Tell state machine to wait until done. SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE); // Tell state machine to go to next state when done. SMWaitEvents(g_autoSM, currState + 1); break; case SMSTATE_STARTED + 6: // Turn left 50 degrees. PIDDriveSetTarget(g_pidDrive, -27.0, 0.0, false, &g_autoSM, EVTTYPE_PIDDRIVE, 2500); // Tell state machine to wait until done. SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE); // Tell state machine to go to next state when done. SMWaitEvents(g_autoSM, currState + 17); break; // POSITION 2 case SMSTATE_STARTED + 7: // Turn left 50 degrees. PIDDriveSetTarget(g_pidDrive, 24.0, 0.0, false, &g_autoSM, EVTTYPE_PIDDRIVE, 2500); // Tell state machine to wait until done. SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE); // Tell state machine to go to next state when done. SMWaitEvents(g_autoSM, currState + 1); break; case SMSTATE_STARTED + 8: // Turn left 50 degrees. PIDDriveSetTarget(g_pidDrive, 0.0, -110.0, false, &g_autoSM, EVTTYPE_PIDDRIVE, 2500); // Tell state machine to wait until done. SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE); // Tell state machine to go to next state when done. SMWaitEvents(g_autoSM, currState + 1); break; case SMSTATE_STARTED + 9: // Turn left 50 degrees. PIDDriveSetTarget(g_pidDrive, 15.0, 0.0, false, &g_autoSM, EVTTYPE_PIDDRIVE, 2500); // Tell state machine to wait until done. SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE); // Tell state machine to go to next state when done. SMWaitEvents(g_autoSM, currState + 1); break; case SMSTATE_STARTED + 10: // Turn left 50 degrees. PIDDriveSetTarget(g_pidDrive, 0.0, -65.0, false, &g_autoSM, EVTTYPE_PIDDRIVE, 2500); // Tell state machine to wait until done. SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE); // Tell state machine to go to next state when done. SMWaitEvents(g_autoSM, currState + 1); break; case SMSTATE_STARTED + 11: // Turn left 50 degrees. PIDDriveSetTarget(g_pidDrive, -40.0, 0.0, false, &g_autoSM, EVTTYPE_PIDDRIVE, 2500); // Tell state machine to wait until done. SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE); // Tell state machine to go to next state when done. SMWaitEvents(g_autoSM, SMSTATE_STARTED + 17); break; // POSITION 3 case SMSTATE_STARTED + 12: // Turn left 50 degrees. PIDDriveSetTarget(g_pidDrive, 30.0, 0.0, false, &g_autoSM, EVTTYPE_PIDDRIVE, 2500); // Tell state machine to wait until done. SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE); // Tell state machine to go to next state when done. SMWaitEvents(g_autoSM, currState + 1); break; case SMSTATE_STARTED + 13: // Turn left 50 degrees. PIDDriveSetTarget(g_pidDrive, 0.0, 90.0, false, &g_autoSM, EVTTYPE_PIDDRIVE, 2500); // Tell state machine to wait until done. SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE); // Tell state machine to go to next state when done. SMWaitEvents(g_autoSM, currState + 1); break; case SMSTATE_STARTED + 14: // Turn left 50 degrees. PIDDriveSetTarget(g_pidDrive, 16.0, 0.0, false, &g_autoSM, EVTTYPE_PIDDRIVE, 2500); // Tell state machine to wait until done. SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE); // Tell state machine to go to next state when done. SMWaitEvents(g_autoSM, currState + 1); break; case SMSTATE_STARTED + 15: // Turn left 50 degrees. PIDDriveSetTarget(g_pidDrive, 0.0, 85.0, false, &g_autoSM, EVTTYPE_PIDDRIVE, 2500); // Tell state machine to wait until done. SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE); // Tell state machine to go to next state when done. SMWaitEvents(g_autoSM, currState + 1); break; case SMSTATE_STARTED + 16: // Turn left 50 degrees. PIDDriveSetTarget(g_pidDrive, -30.0, 0.0, false, &g_autoSM, EVTTYPE_PIDDRIVE, 2500); // Tell state machine to wait until done. SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE); // Tell state machine to go to next state when done. SMWaitEvents(g_autoSM, currState + 1); break; case SMSTATE_STARTED + 17: // Turn left 50 degrees. PIDDriveSetTarget(g_pidDrive, 0.0, 0.0, false, &g_autoSM, EVTTYPE_PIDDRIVE, 1000); // Tell state machine to wait until done. SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE); // Tell state machine to go to next state when done. SMWaitEvents(g_autoSM, currState + 1); break; default: // We are done, stop the state machine. SMStop(g_autoSM); break; } // // Execute all periodic tasks. // PIDDriveTask(g_pidDrive); DriveTask(g_drive); wait1Msec(20); } } } /on = 1; while(on == 1) // while var on (var to close while loop) = 1 { // check sensor value to determine center goal position // then run appropriate case program // move irServo to 130 servoTarget[irServo] = 130; wait10Msec(200); if (_dirAC = HTIRS2readACDir(irSeeker) == 6) { // position 1 on = 0; } // if irSeeker = 6 else // move irServo to 140 { servoTarget[irServo] = 140; } wait10Msec(200); if (_dirAC = HTIRS2readACDir(irSeeker) == 6) { //position 2 on = 0; } // if left ir seeker = 5 and right ir seeker = 2 else { servoTarget[irServo] = 157; } wait10Msec(200); if (_dirAC = HTIRS2readACDir(irSeeker) == 6) { //position 3 on = 0; } }*/

PIDEnc.c


	Code: #pragma config(Hubs, S1, HTMotor, HTMotor, HTServo, none) #pragma config(Sensor, S1, , sensorI2CMuxController) #pragma config(Sensor, S2, irSeeker, sensorI2CCustom) #pragma config(Sensor, S3, htComp, sensorI2CHiTechnicCompass) #pragma config(Sensor, S4, htEOPD, sensorI2CCustom) #pragma config(Motor, mtr_S1_C1_1, rightWheel, tmotorTetrix, openLoop, encoder) #pragma config(Motor, mtr_S1_C1_2, leftWheel, tmotorTetrix, openLoop, reversed, encoder) #pragma config(Motor, mtr_S1_C2_1, rightLifter, tmotorTetrix, PIDControl, reversed) #pragma config(Motor, mtr_S1_C2_2, leftLifter, tmotorTetrix, PIDControl) #pragma config(Servo, srvo_S1_C3_1, servo1, tServoNone) #pragma config(Servo, srvo_S1_C3_2, servo2, tServoNone) #pragma config(Servo, srvo_S1_C3_3, rightDoor, tServoStandard) #pragma config(Servo, srvo_S1_C3_4, leftDoor, tServoStandard) #pragma config(Servo, srvo_S1_C3_5, goalHook, tServoStandard) #pragma config(Servo, srvo_S1_C3_6, irServo, tServoStandard) //!!Code automatically generated by 'ROBOTC' configuration wizard !!// // include files #include "ftclib/trcdefs.h" #include "ftclib/dbgtrace.h" #include "ftclib/sm.h" #include "ftclib/drive.h" #include "ftclib/pidctrl.h" #include "ftclib/piddrive.h" #include "JoystickDriver.c" // define distance with encoders #define ENC_KP 1.7 // change this to meet distance target #define ENC_KI 0.0015 #define ENC_KD 0.5 #define ENC_TOLERANCE 1.0 #define ENC_SETTLING 200 // define turns with encoder #define TURN_KP 1.5 // change this to meet turn target #define TURN_KI 0.0 #define TURN_KD 0.5 #define TURN_TOLERANCE 1.8 #define TURN_SETTLING 200 #define CLICKS_PER_INCH 118.5 // change this to adjust actual distance #define CLICKS_PER_DEGREE 17.0 // change this to adjust actual degree #define EVTTYPE_PIDDRIVE (EVTTYPE_NONE + 1) DRIVE g_drive; //create the drive object. PIDCTRL g_yPidCtrl; //create the Y PID controller PIDCTRL g_turnPidCtrl; //create the turn PID controller PIDDRIVE g_pidDrive; //create the PID drive object SM g_autoSM; //create the autonomous state machine object // calculate all the math float PIDCtrlGetInput(PIDCTRL &pidCtrl) { float inputValue = 0.0; if (IsSameReference(pidCtrl, g_yPidCtrl)) { inputValue = (nMotorEncoder[leftWheel] + nMotorEncoder[rightWheel])/2.0/CLICKS_PER_INCH; } else if (IsSameReference(pidCtrl, g_turnPidCtrl)) { inputValue = (nMotorEncoder[leftWheel] - nMotorEncoder[rightWheel])/2.0/CLICKS_PER_DEGREE; } return inputValue; } task main() { waitForStart(); // Initialize the drive object. DriveInit(g_drive, leftWheel, rightWheel); // Initialize the yPidCtrl object. PIDCtrlInit(g_yPidCtrl, ENC_KP, ENC_KI, ENC_KD, ENC_TOLERANCE, ENC_SETTLING); // Initialize the turnPidCtrl object. PIDCtrlInit(g_turnPidCtrl, TURN_KP, TURN_KI, TURN_KD, TURN_TOLERANCE, TURN_SETTLING); // Initialize the pidDrive object. PIDDriveInit(g_pidDrive, g_drive, g_yPidCtrl, g_turnPidCtrl); // Initialize the autonomous state machine and start it. SMInit(g_autoSM); SMStart(g_autoSM); while (true) { // display debug info on nxt displayTextLine(0, "state=%d", SMGetState(g_autoSM)); PIDCtrlDisplayInfo(1, g_yPidCtrl); PIDCtrlDisplayInfo(3, g_turnPidCtrl); // Check if state machine is ready. if (SMIsReady(g_autoSM)) { int currState = SMGetState(g_autoSM); switch (currState) { // position 1 /case SMSTATE_STARTED: // Drive forward for 24 inches. PIDDriveSetTarget(g_pidDrive, 24.0, 0.0, false, &g_autoSM, EVTTYPE_PIDDRIVE, 2500); // Tell state machine to wait until done. SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE); // Tell state machine to go to next state when done. SMWaitEvents(g_autoSM, currState + 1); break; case SMSTATE_STARTED + 1: // Turn left 75 degrees. PIDDriveSetTarget(g_pidDrive, 0.0, -75.0, false, &g_autoSM, EVTTYPE_PIDDRIVE, 2500); // Tell state machine to wait until done. SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE); // Tell state machine to go to next state when done. SMWaitEvents(g_autoSM, currState + 1); break; case SMSTATE_STARTED + 2: // Drive forward 30 inches. PIDDriveSetTarget(g_pidDrive, 30.0, 0.0, false, &g_autoSM, EVTTYPE_PIDDRIVE, 2500); // Tell state machine to wait until done. SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE); // Tell state machine to go to next state when done. SMWaitEvents(g_autoSM, currState + 1); break; case SMSTATE_STARTED + 3: // Turn left 55 degrees. PIDDriveSetTarget(g_pidDrive, 0.0, -55.0, false, &g_autoSM, EVTTYPE_PIDDRIVE, 2500); // Tell state machine to wait until done. SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE); // Tell state machine to go to next state when done. SMWaitEvents(g_autoSM, currState + 1); break; case SMSTATE_STARTED + 4: // Turn left 50 degrees. PIDDriveSetTarget(g_pidDrive, -27.0, 0.0, false, &g_autoSM, EVTTYPE_PIDDRIVE, 2500); // Tell state machine to wait until done. SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE); // Tell state machine to go to next state when done. SMWaitEvents(g_autoSM, currState + 1); break; ///////////////////////////////////////////////////////////////////////////////////////////////////////// /case SMSTATE_STARTED: // Turn left 50 degrees. PIDDriveSetTarget(g_pidDrive, 24.0, 0.0, false, &g_autoSM, EVTTYPE_PIDDRIVE, 2500); // Tell state machine to wait until done. SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE); // Tell state machine to go to next state when done. SMWaitEvents(g_autoSM, currState + 1); break; case SMSTATE_STARTED + 1: // Turn left 50 degrees. PIDDriveSetTarget(g_pidDrive, 0.0, -110.0, false, &g_autoSM, EVTTYPE_PIDDRIVE, 2500); // Tell state machine to wait until done. SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE); // Tell state machine to go to next state when done. SMWaitEvents(g_autoSM, currState + 1); break; case SMSTATE_STARTED + 2: // Turn left 50 degrees. PIDDriveSetTarget(g_pidDrive, 15.0, 0.0, false, &g_autoSM, EVTTYPE_PIDDRIVE, 2500); // Tell state machine to wait until done. SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE); // Tell state machine to go to next state when done. SMWaitEvents(g_autoSM, currState + 1); break; case SMSTATE_STARTED + 3: // Turn left 50 degrees. PIDDriveSetTarget(g_pidDrive, 0.0, -65.0, false, &g_autoSM, EVTTYPE_PIDDRIVE, 2500); // Tell state machine to wait until done. SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE); // Tell state machine to go to next state when done. SMWaitEvents(g_autoSM, currState + 1); break; case SMSTATE_STARTED + 4: // Turn left 50 degrees. PIDDriveSetTarget(g_pidDrive, -40.0, 0.0, false, &g_autoSM, EVTTYPE_PIDDRIVE, 2500); // Tell state machine to wait until done. SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE); // Tell state machine to go to next state when done. SMWaitEvents(g_autoSM, currState + 1); break; ///////////////////////////////////////////////////////////////////////////////////////////////////////// // position 3 /case SMSTATE_STARTED: // Turn left 50 degrees. PIDDriveSetTarget(g_pidDrive, 30.0, 0.0, false, &g_autoSM, EVTTYPE_PIDDRIVE, 2500); // Tell state machine to wait until done. SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE); // Tell state machine to go to next state when done. SMWaitEvents(g_autoSM, currState + 1); break; case SMSTATE_STARTED + 1: // Turn left 50 degrees. PIDDriveSetTarget(g_pidDrive, 0.0, 90.0, false, &g_autoSM, EVTTYPE_PIDDRIVE, 2500); // Tell state machine to wait until done. SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE); // Tell state machine to go to next state when done. SMWaitEvents(g_autoSM, currState + 1); break; case SMSTATE_STARTED + 2: // Turn left 50 degrees. PIDDriveSetTarget(g_pidDrive, 16.0, 0.0, false, &g_autoSM, EVTTYPE_PIDDRIVE, 2500); // Tell state machine to wait until done. SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE); // Tell state machine to go to next state when done. SMWaitEvents(g_autoSM, currState + 1); break; case SMSTATE_STARTED + 3: // Turn left 50 degrees. PIDDriveSetTarget(g_pidDrive, 0.0, 85.0, false, &g_autoSM, EVTTYPE_PIDDRIVE, 2500); // Tell state machine to wait until done. SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE); // Tell state machine to go to next state when done. SMWaitEvents(g_autoSM, currState + 1); break; case SMSTATE_STARTED + 4: // Turn left 50 degrees. PIDDriveSetTarget(g_pidDrive, -30.0, 0.0, false, &g_autoSM, EVTTYPE_PIDDRIVE, 2500); // Tell state machine to wait until done. SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE); // Tell state machine to go to next state when done. SMWaitEvents(g_autoSM, currState + 1); break;/ case SMSTATE_STARTED: // Turn left 50 degrees. PIDDriveSetTarget(g_pidDrive, 72.0, 0.0, false, &g_autoSM, EVTTYPE_PIDDRIVE, 2500); // Tell state machine to wait until done. SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE); // Tell state machine to go to next state when done. SMWaitEvents(g_autoSM, currState + 1); break; case SMSTATE_STARTED + 1: // Turn left 50 degrees. PIDDriveSetTarget(g_pidDrive, 0.0, -90.0, false, &g_autoSM, EVTTYPE_PIDDRIVE, 2500); // Tell state machine to wait until done. SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE); // Tell state machine to go to next state when done. SMWaitEvents(g_autoSM, currState + 1); break; case SMSTATE_STARTED + 2: // Turn left 50 degrees. PIDDriveSetTarget(g_pidDrive, 25.0, 0.0, false, &g_autoSM, EVTTYPE_PIDDRIVE, 2500); // Tell state machine to wait until done. SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE); // Tell state machine to go to next state when done. SMWaitEvents(g_autoSM, currState + 1); break; case SMSTATE_STARTED + 3: // Turn left 50 degrees. PIDDriveSetTarget(g_pidDrive, 0.0, 90.0, false, &g_autoSM, EVTTYPE_PIDDRIVE, 2500); // Tell state machine to wait until done. SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE); // Tell state machine to go to next state when done. SMWaitEvents(g_autoSM, currState + 1); break; case SMSTATE_STARTED + 4: // Turn left 50 degrees. PIDDriveSetTarget(g_pidDrive, 30.0, 0.0, false, &g_autoSM, EVTTYPE_PIDDRIVE, 2500); // Tell state machine to wait until done. SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE); // Tell state machine to go to next state when done. SMWaitEvents(g_autoSM, currState + 1); break; case SMSTATE_STARTED + 5: // Turn left 50 degrees. PIDDriveSetTarget(g_pidDrive, 50.0, 0.0, false, &g_autoSM, EVTTYPE_PIDDRIVE, 2500); // Tell state machine to wait until done. SMAddWaitEvent(g_autoSM, EVTTYPE_PIDDRIVE); // Tell state machine to go to next state when done. SMWaitEvents(g_autoSM, currState + 1); break; default: // We are done, stop the state machine. SMStop(g_autoSM); break; } } // // Execute all periodic tasks. // PIDDriveTask(g_pidDrive); DriveTask(g_drive); wait1Msec(20); } }

PID Control

Who is online