Author |
Message |
BTB1337
Rookie
Joined: Sat Jan 31, 2015 1:14 am Posts: 38
|
 Re: PID Control
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); } }
|  |  |  |  |
|
Tue Feb 24, 2015 12:49 pm |
|
 |
MHTS
Guru
Joined: Sun Nov 15, 2009 5:46 am Posts: 1523
|
 Re: PID Control
You got the right idea but you have a few problems in your code. I have corrected those problems with the following code. First, the whole point of using a state machine is to support multi-tasking. In order to do that, you absolutely cannot have any loop or wait in the state machine loop. Putting while-loops and wait10Msec statements in the state machine while-loop violates that. All the while-loop code is basically trying to determine the center goal position. You can do it outside of the main state machine while loop. I have optimized that code with a for-loop and position tables. Secondly, I moved all the initialization code into initializeRobot. All initialization should be done before waitForStart so it doesn't waste any of the precious autonomous time (30 seconds). In fact, I am even tempted to move the position detection for-loop into initializeRobot because the code is changing the servo angle and waiting for a whole 2 seconds for each of the 3 positions. This wasted 6 of the 30 seconds. But doing that may not work because the position detection code may run prematurely before the field setup people change the center goal position. So I left it after waitForStart but you should seriously consider not waiting that long. I'd bet you really meant 200 msec not 2 seconds. Thirdly, when you start the state machine, you can specify the starting state, so you don't have to make a start state that do a bogus SMWaitEvent just to go to the correct state according to the center goal position.  |  |  |  | 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 "JoystickDriver.c" #include "RobotCDrivers/Drivers/hitechnic-irseeker-v2.h" #include "ftclib/trcdefs.h" #include "ftclib/dbgtrace.h" #include "ftclib/sm.h" #include "ftclib/drive.h" #include "ftclib/pidctrl.h" #include "ftclib/piddrive.h" // 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) #define NUM_POSITIONS 3
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 int g_irServoAngleTable[NUM_POSITIONS] = {130, 140, 157}; int g_startStateTable[NUM_POSITIONS] = {SMSTATE_STARTED + 100, SMSTATE_STARTED + 200, SMSTATE_STARTED + 300}; // 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; } void initializeRobot() { servoTarget[rightDoor] = 0; servoTarget[leftDoor] = 180; servoTarget[irServo] = 130;
// 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); } task main() { int startState = SMSTATE_STARTED;
initializeRobot();
waitForStart();
for (int i = 0; i < NUM_POSITIONS; i++) { servoTarget[irServo] = g_irServoAngleTable[i]; wait10Msec(200); // Do you really want to wait 2 whole seconds??? if (HTIRS2readACDir(irSeeker) == 6) { startState = g_startStateTable[i]; break; } }
SMStart(g_autoSM, startState);
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 + 100: // 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 + 101: // 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 + 102: // 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 + 103: // 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 + 104: // 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 + 400); break; // POSITION 2 case SMSTATE_STARTED + 200: // 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 + 201: // 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 + 202: // 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 + 203: // 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 + 204: // 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 + 400); break; // POSITION 3 case SMSTATE_STARTED + 300: // 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 + 301: // 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 + 302: // 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 + 303: // 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 + 304: // 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 + 400); break; case SMSTATE_STARTED + 400: // 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); } }
|  |  |  |  |
|
Tue Feb 24, 2015 4:01 pm |
|
 |
MHTS
Guru
Joined: Sun Nov 15, 2009 5:46 am Posts: 1523
|
 Re: PID Control
BTW, I just looked at the code more carefully, what is the purpose of SMSTATE_STARTED + 400? It is calling PIDDriveSetTarget with 0.0 yTarget and 0.0 turn target. In other words, this state does absolutely nothing.
|
Tue Feb 24, 2015 4:11 pm |
|
 |
BTB1337
Rookie
Joined: Sat Jan 31, 2015 1:14 am Posts: 38
|
 Re: PID Control
thanks so much, that makes a lot more sense, i'm just glad I was able to get the basic idea, i was concerned I was way way way off haha. State 400 was there because origionally i thought at the end of the state at say in the origional code at the end of position one, end of state 4, it would jump down to state 400 which would stop the robot and from there would move on to the ending break...if that makes sense?
|
Wed Feb 25, 2015 12:28 pm |
|
 |
MHTS
Guru
Joined: Sun Nov 15, 2009 5:46 am Posts: 1523
|
 Re: PID Control
If stopping the robot at the end is your goal, that's not necessary because if the robot did not stop, it would have stayed in a state waiting for PID to complete and would not go to state + 400 anyway. If you really want to make sure the robot stops at the end, you want to call PIDDriveReset instead of PIDDriveSetTarget of 0.0. Also remember to change all the goto SMSTATE_STARTED + 400 to currState + 1 at the end of all the goal positions.
|
Wed Feb 25, 2015 1:50 pm |
|
|
Who is online |
Users browsing this forum: No registered users and 2 guests |
|
You cannot post new topics in this forum You cannot reply to topics in this forum You cannot edit your posts in this forum You cannot delete your posts in this forum You cannot post attachments in this forum
|
|