This is my first time coding a PID loop and im not sure if this code will work for autonomous. I still have to mess with the Kp, Ki, and Kd values.
#pragma config(Sensor, dgtl1, , sensorQuadEncoder)
#pragma config(Sensor, dgtl3, , sensorQuadEncoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
/*---------------------------------------------------------------------------*/
/* */
/* Description: Competition template for VEX EDR */
/* */
/*---------------------------------------------------------------------------*/
// This code is for the VEX cortex platform
#pragma platform(VEX2)
// Select Download method as "competition"
#pragma competitionControl(Competition)
//Main competition background code...do not modify!
#include "Vex_Competition_Includes.c"
/*---------------------------------------------------------------------------*/
/* Pre-Autonomous Functions */
/* */
/* You may want to perform some actions before the competition starts. */
/* Do them in the following function. You must return from this function */
/* or the autonomous and usercontrol tasks will not be started. This */
/* function is only called once after the cortex has been powered on and */
/* not every time that the robot is disabled. */
/*---------------------------------------------------------------------------*/
float Kp = 0;
float Ki = 0;
float Kd = 0;
void moveForward(float distance)
{
float errorLeft;
float errorRight;
float errorTotalLeft;
float errorTotalRight;
float lastErrorLeft;
float lastErrorRight;
float proportionLeft;
float proportionRight;
float integralLeft;
float integralRight;
float integralActiveZone = 57;
float derivativeLeft;
float derivativeRight;
float speedLeft;
float speedRight;
//convert distance in inches to raw values
errorLeft = distance * 57.2967795;
errorRight = distance * 57.2967795;
//calculates new error based on distance left from objective
errorLeft = errorLeft - SensorValue(dgtl1);
errorRight = errorRight - SensorValue(dgtl3);
if(errorLeft < integralActiveZone && errorLeft != 0)
{
errorTotalLeft += errorLeft;
}
else
{
errorTotalLeft = 0;
}
if(errorRight < integralActiveZone && errorRight != 0)
{
errorTotalRight += errorRight;
}
else
{
errorTotalRight = 0;
}
//proportion values
proportionLeft = errorLeft * Kp;
proportionRight = errorRight * Kp;
//integral values
integralLeft = errorTotalLeft * Ki;
integralRight = errorTotalRight * Ki;
//derivative values
derivativeLeft = (errorLeft - lastErrorLeft) * Kd;
derivativeRight = (errorRight - lastErrorRight) * Kd;
//add P, I, and D to get motor speed
speedLeft = (proportionLeft + integralLeft + derivativeLeft);
speedRight = (proportionRight + integralRight + derivativeRight);
//motors move at speed found
motor(port1) = speedLeft;
motor(port3) = speedRight;
//make error into lastError after loop
lastErrorLeft = errorLeft;
lastErrorRight = errorRight;
wait1Msec(25); //let motors and cortex catch up
}
void moveBackward(float distance)
{
float errorLeft;
float errorRight;
float errorTotalLeft;
float errorTotalRight;
float lastErrorLeft;
float lastErrorRight;
float proportionLeft;
float proportionRight;
float integralLeft;
float integralRight;
float integralActiveZone = -57;
float derivativeLeft;
float derivativeRight;
float speedLeft;
float speedRight;
//convert distance in inches to raw values
errorLeft = distance * -57.2967795;
errorRight = distance * -57.2967795;
//calculates new error based on distance left from objective
errorLeft = errorLeft - SensorValue(dgtl1);
errorRight = errorRight - SensorValue(dgtl3);
if(errorLeft > integralActiveZone && errorLeft != 0)
{
errorTotalLeft += errorLeft;
}
else
{
errorTotalLeft = 0;
}
if(errorRight > integralActiveZone && errorRight != 0)
{
errorTotalRight += errorRight;
}
else
{
errorTotalRight = 0;
}
//proportion values
proportionLeft = errorLeft * Kp;
proportionRight = errorRight * Kp;
//integral values
integralLeft = errorTotalLeft * Ki;
integralRight = errorTotalRight * Ki;
//derivative values
derivativeLeft = (errorLeft - lastErrorLeft) * Kd;
derivativeRight = (errorRight - lastErrorRight) * Kd;
//add P, I, and D to get motor speed
speedLeft = (proportionLeft + integralLeft + derivativeLeft);
speedRight = (proportionRight + integralRight + derivativeRight);
//motors move at speed found
motor(port1) = speedLeft;
motor(port3) = speedRight;
//make error into lastError after loop
lastErrorLeft = errorLeft;
lastErrorRight = errorRight;
wait1Msec(25); //let motors and cortex catch up
}
void turnRight(float degrees)
{
float errorLeft;
float errorRight;
float errorTotalLeft;
float errorTotalRight;
float lastErrorLeft;
float lastErrorRight;
float proportionLeft;
float proportionRight;
float integralLeft;
float integralRight;
float integralActiveZone = -57;
float derivativeLeft;
float derivativeRight;
float speedLeft;
float speedRight;
//right side back, left side front
//627.2 ticks per revolution
//how many ticks is in 1 degree
errorLeft = (degrees/360) * 79.9718928869;
errorRight = (degrees/360) * -79.9718928869;
//calculates new error based on distance left from objective
errorLeft = errorLeft - SensorValue(dgtl1);
errorRight = errorRight - SensorValue(dgtl3);
if(errorLeft < integralActiveZone && errorLeft != 0)
{
errorTotalLeft += errorLeft;
}
else
{
errorTotalLeft = 0;
}
if(errorRight > integralActiveZone && errorRight != 0)
{
errorTotalRight += errorRight;
}
else
{
errorTotalRight = 0;
}
//proportion values
proportionLeft = errorLeft * Kp;
proportionRight = errorRight * Kp;
//integral values
integralLeft = errorTotalLeft * Ki;
integralRight = errorTotalRight * Ki;
//derivative values
derivativeLeft = (errorLeft - lastErrorLeft) * Kd;
derivativeRight = (errorRight - lastErrorRight) * Kd;
//add P, I, and D to get motor speed
speedLeft = (proportionLeft + integralLeft + derivativeLeft);
speedRight = (proportionRight + integralRight + derivativeRight);
//motors move at speed found
motor(port1) = speedLeft;
motor(port3) = speedRight;
//make error into lastError after loop
lastErrorLeft = errorLeft;
lastErrorRight = errorRight;
wait1Msec(25); //let motors and cortex catch up
}
void turnLeft(float degrees)
{
float errorLeft;
float errorRight;
float errorTotalLeft;
float errorTotalRight;
float lastErrorLeft;
float lastErrorRight;
float proportionLeft;
float proportionRight;
float integralLeft;
float integralRight;
float integralActiveZone = -57;
float derivativeLeft;
float derivativeRight;
float speedLeft;
float speedRight;
//left side back, right side front
//627.2 ticks per revolution
//how many ticks is in 1 degree
errorLeft = (degrees/360) * -79.9718928869;
errorRight = (degrees/360) * 79.9718928869;
//calculates new error based on distance left from objective
errorLeft = errorLeft - SensorValue(dgtl1);
errorRight = errorRight - SensorValue(dgtl3);
if(errorLeft > integralActiveZone && errorLeft != 0)
{
errorTotalLeft += errorLeft;
}
else
{
errorTotalLeft = 0;
}
if(errorRight < integralActiveZone && errorRight != 0)
{
errorTotalRight += errorRight;
}
else
{
errorTotalRight = 0;
}
//proportion values
proportionLeft = errorLeft * Kp;
proportionRight = errorRight * Kp;
//integral values
integralLeft = errorTotalLeft * Ki;
integralRight = errorTotalRight * Ki;
//derivative values
derivativeLeft = (errorLeft - lastErrorLeft) * Kd;
derivativeRight = (errorRight - lastErrorRight) * Kd;
//add P, I, and D to get motor speed
speedLeft = (proportionLeft + integralLeft + derivativeLeft);
speedRight = (proportionRight + integralRight + derivativeRight);
//motors move at speed found
motor(port1) = speedLeft;
motor(port3) = speedRight;
//make error into lastError after loop
lastErrorLeft = errorLeft;
lastErrorRight = errorRight;
wait1Msec(25); //let motors and cortex catch up
}
void pre_auton()
{
// Set bStopTasksBetweenModes to false if you want to keep user created tasks
// running between Autonomous and Driver controlled modes. You will need to
// manage all user created tasks if set to false.
bStopTasksBetweenModes = true;
SensorValue(dgtl1) = 0;
SensorValue(dgtl3) = 0;
// Set bDisplayCompetitionStatusOnLcd to false if you don't want the LCD
// used by the competition include file, for example, you might want
// to display your team name on the LCD in this function.
// bDisplayCompetitionStatusOnLcd = false;
// All activities that occur before the competition starts
// Example: clearing encoders, setting servo positions, ...
}
/*---------------------------------------------------------------------------*/
/* */
/* Autonomous Task */
/* */
/* This task is used to control your robot during the autonomous phase of */
/* a VEX Competition. */
/* */
/* You must modify the code to add your own robot specific commands here. */
/*---------------------------------------------------------------------------*/
task autonomous()
{
moveForward(20);
moveBackward(13);
turnRight(90);
moveForward(20);
moveBackward(20);
turnLeft(90);
}