int main()
{
while (true) {
//thrust PID
error = setpoint - getHeight();
integral = integral + error * dt;
derivative = (error - previous_error);
thrust = Kp * error + Ki * integral + Kd * derivative;
//Pitch PID
error_P = 0 - getPitch();
integral_P = integral_P + error_P * dt;
derivative_P = (error_P - pre_error_P);
pitch = Kp * error_P + Ki * integral_P + Kd * derivative_P;
//Roll PID
error_R = 0 - getRoll();
integral_R = integral_R + error_R * dt;
derivative_R = (error_R - pre_error_R);
roll = Kp * error_R + Ki * integral_P + Kd * derivative_R;
//Motor controls
if (thrust > 255) thrust = 255;
else if (thrust < 0) thrust = 0;
setFR(makeValid(thrust + pitch + roll)); // front right
setFL(makeValid(thrust + pitch - roll)); // front left
setBR(makeValid(thrust - pitch + roll)); // back right
setBL(makeValid(thrust - pitch - roll)); // back left
//set previous errors
previous_error = error;
pre_error_P = error_P;
pre_error_R = error_R;
Sleep(1000 * dt);
}
}
//go sideways for distance(in degrees) in a straight line at heading goal(front of robot)
//distance is always a positive integer
//goal should be 0 coming out of a turn so it finishes adjustments it couldn't do because of the motor cutoff
//alternately the goal could be the current gyro heading
//base power is positive or negative depending on desired direction-negative = left; positive = right
void sideways(int distance, int goal, int basePower, int cutOff)
{
//adjusts distance
distance = abs(distance);
distance = (int)(distance*(360/sideConversion));
int direction;
//sets direction for use later
if(basePower <= 0)
{
direction = -1;
}
else
{
direction = 1;
}
//error variables for use later
float error;
float oldError = 0;
float change = 0;
//resets variables for use later
proportion = 0;
integral = 0;
derivative = 0;
//set constants for PID
float Kp = 0.5;
float Ki = 0.054;
float Kd = 1.5;
//prepares goal for use with gyro
goal *= 10;
//stores gyro value
int gyroValue;
//will loop while the average of the sides is less than the direction
while((direction * NMotorEncoder[BL] + direction * (-1*NMotorEncoder[BR]))/2 < distance)
{
//preparing error...mostly magic
gyroValue = SensorValue[gyro];
gyroValue = centerGyro(goal, gyroValue);
error = adjustGyro(gyroValue);
change = PID(Kp, Ki, Kd, error, oldError);
change = change * -1;
weightMechanum(basePower, 0, change);
setFR(FRPower, cutOff);
setFL(FLPower, cutOff);
setBL(BLPower, cutOff);
setBR(BRPower, cutOff);
//sets the current error to be the old error for the next iteration
oldError = error;
//waits so that calculations are at even intervals
wait1Msec(50);
}
stopDrive();
}
void MechanumDrive(int bottomThresh, int topThresh, int lowGear, int cutOff)
{
//these variables allow you to change which channels conrtol different movements
if(controllerState == 0)
{
XVal = 0; //left/right
ZVal = vexRT[Ch3]; //forward/back
RotateVal = vexRT[Ch1]; //rotational
}
else if(controllerState == 1)
{
XVal = 0;
ZVal = vexRT[Ch3Xmtr2];
RotateVal = vexRT[Ch1Xmtr2];
}
/*
accelChange = abs(ZVal - oldZVal);
//if accel is in progress continue and aim for ZVal always
if(accel)
{
ZVal = accelRun(ZVal);
}
//if the ZVal has changed enough turn on accel
if(abs(ZVal - oldZVal) >= accelCutOff)
{
if(ZVal > oldZVal)
{
accelDirection = 1
}
else
{
accelDirection = -1
}
ZVal = accelRun(ZVal);
accel = true;
}
oldZVal = ZVal;
*/
//these manipulate the channel values going into the main function
//*see top and bottom thresh above
if(abs(ZVal)< bottomThresh)
{
ZVal = 0;
}
else if(abs(ZVal) < topThresh)
{
ZVal = ZVal/lowGear;
}
if(abs(XVal)< bottomThresh)
{
XVal = 0;
}
else if(abs(XVal) < topThresh)
{
XVal = XVal/lowGear;
}
if(abs(RotateVal)< bottomThresh)
{
RotateVal = 0;
}
else if(abs(RotateVal) < topThresh)
{
RotateVal = RotateVal/lowGear;
}
weightMechanum(XVal, ZVal, RotateVal);
//Set powers to motors
setFR(FRPower, cutOff);
setFL(FLPower, cutOff);
setBL(BLPower, cutOff);
setBR(BRPower, cutOff);
}
int main()
{
//Declare variables for equation use
//Sensor Variables
int8_t height1;
int8_t pitch1;
int8_t roll1;
uint8_t height2;
int8_t pitch2;
int8_t roll2;
//Target Variables
int8_t pitchTar = 0;
int8_t rollTar = 0;
uint8_t heightTar = 127;
//Rate of change variables
double derPitch;
double derRoll;
double derHeight;
//Calculated Thrust Variables
double pitchThrust;
double rollThrust;
double heightThrust;
double adjustHeightThrust;
//Motor thrust before height adjustment variables
double FRTNoAdjustment;
double FLTNoAdjustment;
double BRTNoAdjustment;
double BLTNoAdjustment;
//Motor thrust after height adjustment variablefs
uint8_t FRT;
uint8_t FLT;
uint8_t BRT;
uint8_t BLT;
//Detect Pitch, Roll, and Height and store as variables
pitch1 = getPitch();
roll1 = getRoll();
height1 = getHeight();
//Calculate initial motor strength guess based on Pitch, Roll, and Height
FRT = (int)pitch1 + (int)roll1 + (int)heightTar - (int)height1;
FLT = (int)pitch1 - (int)roll1 + (int)heightTar - (int)height1;
BRT = -(int)pitch1 + (int)roll1 + (int)heightTar - (int)height1;
BLT = -(int)pitch1 - (int)roll1 + (int)heightTar - (int)height1;
//Set the motor strength
setFR(FRT);
setFL(FLT);
setBR(BRT);
setBL(BLT);
//Delay
Sleep(1000);
//Infinite loop
while (true)
{
//Detect position again
pitch2 = getPitch();
roll2 = getRoll();
height2 = getHeight();
//Calculate rate of change based on thrust and pitch
if ((int)FRT + (int)FLT - (int)BRT - (int)BLT != 0)
{
derPitch = ((int)pitch2 - (int)pitch1) / ((int)FRT + (int)FLT - (int)BRT - (int)BLT);
if (derPitch != 0)
{
//Calculate desired thrust based on rate of change in pitch
pitchThrust = ((int)pitchTar - (int)pitch2) / derPitch;
}
}
//Calculate rate of change based on thrust and roll
if ((int)FRT + (int)BRT - (int)FLT - (int)BLT != 0)
{
derRoll = ((int)roll2 - (int)roll1) / ((int)FRT + (int)BRT - (int)FLT - (int)BLT);
if (derRoll != 0)
{
//Calculate desired thrust based on rate of change in roll
rollThrust = ((int)rollTar - (int)roll2) / derRoll;
}
}
//Calculate rate of change based on thrust and height
if ((int)FRT + (int)FLT + (int)BRT + (int)BLT != 0)
{
derHeight = ((int)height2 - (int)height1) / ((int)FRT + (int)FLT + (int)BRT + (int)BLT);
if (derHeight != 0)
{
//Calculate desired thrust based on rate of change in height
heightThrust = ((int)heightTar - (int)height2) / derHeight;
}
}
//Calculate what thrust would be with only pitch and roll taken in to account
FRTNoAdjustment = pitchThrust + rollThrust;
FLTNoAdjustment = pitchThrust - rollThrust;
BRTNoAdjustment = -pitchThrust + rollThrust;
BLTNoAdjustment = -pitchThrust - rollThrust;
//Calculate change in thrust required to get to target height
adjustHeightThrust = (heightThrust - (FRTNoAdjustment + FLTNoAdjustment + BRTNoAdjustment + BLTNoAdjustment)) / 4;
//Calculate thrust based on pitch roll and height
FRT = pitchThrust + rollThrust + adjustHeightThrust;
FLT = pitchThrust - rollThrust + adjustHeightThrust;
BRT = -pitchThrust + rollThrust + adjustHeightThrust;
BLT = -pitchThrust - rollThrust + adjustHeightThrust;
//Set the motor strength
setFR(FRT);
setFL(FLT);
setBR(BRT);
setBL(BLT);
//Save old positionf
pitch1 = pitch2;
roll1 = roll2;
height1 = height2;
//Delay
Sleep(1000);
}
return 0;
}
