void TeleopPeriodic() {
if(m_driver->GetRawButton(BUTTON_LB)) {
// PYRAMID
m_PIDController->SetSetpoint(PLATE_PYRAMID_THREE_POINT);
m_PIDController->Enable();
} else if(m_driver->GetRawButton(BUTTON_RB)) {
// FEEDER
m_PIDController->SetSetpoint(PLATE_FEEDER_THREE_POINT);
m_PIDController->Enable();
} else if(m_driver->GetRawAxis(TRIGGERS) > 0.5) {
m_PIDController->SetSetpoint(PLATE_TEN_POINT_CLIMB);
m_PIDController->Enable();
} else {
// MANUAL CONTROL
m_PIDController->Disable();
m_plate1->Set(-deadband(m_driver->GetRawAxis(LEFT_Y)));
m_plate2->Set(-deadband(m_driver->GetRawAxis(LEFT_Y)));
}
// ----- PRINT -----
SmartDashboard::PutNumber("Plate Position: ", m_plateSensor->GetVoltage());
SmartDashboard::PutNumber("PID GET: ", m_plateSensor->PIDGet());
} // TeleopPeriodic()
task main()
{
float leftFront, leftBack, rightFront, rightBack; // motors
/***** BEGIN Mecanum Arcade Drive Test *****/
init();
StartTask(gyro_calibrate, 8);
StartTask(displaySmartDiags, 255);
if (bCompetitionMode) {waitForStart();}
while (true)
{
/***** Proportional Motor Control *****/
getJoystickSettings(joystick); //get all joystick statuses
mecanum_arcade(deadband(k_deadband,joystick.joy1_y1),
deadband(k_deadband,joystick.joy1_x1),
deadband(k_deadband,joystick.joy1_x2),
leftFront, rightFront, leftBack, rightBack);
motor[Lf] = leftFront;
motor[Rf] = rightFront;
motor[Lb] = leftBack;
motor[Rb] = rightBack;
nxtDisplayCenteredBigTextLine(6, "%.2f", gyro_getheading());
nxtDisplayCenteredTextLine(5, "%.2f", gyro_getrotspeed());
if(joy1Btn(4) == 1) { gyro_reset(); }
while(nNxtButtonPressed == kEnterButton) { gyro_reset(); }
}
}
void setDrive() // reads Joystick, calculates drive motor values and sends values to motors - used in driver control
{
int driveJoystickY = deadband(vexRT[Ch3], DRIVE_DEADBAND_THRESHOLD); // read from the left josytick Y-axis (channel 3) and deadband value
int driveJoystickX = deadband(vexRT[Ch4], DRIVE_DEADBAND_THRESHOLD); // read from the left josytick X-axis (channel 4) and deadband value
int rightMotorValue = driveJoystickY - driveJoystickX; // arcade drive left side value
int leftMotorValue = driveJoystickY + driveJoystickX; // arcade drive right side value
setDriveMotors(rightMotorValue, leftMotorValue); // send the calculated motor values to the drive sides
}
/**
* update attitude
*
* @param
* void
*
* @return
* void
*
*/
void attitudeUpdate(){
float yrpAttitude[3];
float pryRate[3];
float xyzAcc[3];
float xComponent[3];
float yComponent[3];
float zComponent[3];
float xyzMagnet[3];
#if CHECK_ATTITUDE_UPDATE_LOOP_TIME
struct timeval tv_c;
static struct timeval tv_l;
unsigned long timeDiff=0;
gettimeofday(&tv_c,NULL);
timeDiff=GET_USEC_TIMEDIFF(tv_c,tv_l);
_DEBUG(DEBUG_NORMAL,"attitude update duration=%ld us\n",timeDiff);
UPDATE_LAST_TIME(tv_c,tv_l);
#endif
getYawPitchRollInfo(yrpAttitude, pryRate, xyzAcc, xComponent, yComponent, zComponent,xyzMagnet);
setYaw(yrpAttitude[0]);
setRoll(yrpAttitude[1]);
setPitch(yrpAttitude[2]);
setYawGyro(-pryRate[2]);
setPitchGyro(pryRate[0]);
setRollGyro(-pryRate[1]);
setXAcc(xyzAcc[0]);
setYAcc(xyzAcc[1]);
setZAcc(xyzAcc[2]);
setXGravity(zComponent[0]);
setYGravity(zComponent[1]);
setZGravity(zComponent[2]);
setVerticalAcceleration(deadband((getXAcc() * zComponent[0] + getYAcc() * zComponent[1]
+ getZAcc() * zComponent[2] - 1.f) * 100.f,3.f) );
setXAcceleration(deadband((getXAcc() * xComponent[0] + getYAcc() * xComponent[1]
+ getZAcc() * xComponent[2]) * 100.f,3.f) );
setYAcceleration(deadband((getXAcc() * yComponent[0] + getYAcc() * yComponent[1]
+ getZAcc() * yComponent[2]) * 100.f,3.f) );
_DEBUG(DEBUG_ATTITUDE,
"(%s-%d) ATT: Roll=%3.3f Pitch=%3.3f Yaw=%3.3f\n", __func__,
__LINE__, getRoll(), getPitch(), getYaw());
_DEBUG(DEBUG_GYRO,
"(%s-%d) GYRO: Roll=%3.3f Pitch=%3.3f Yaw=%3.3f\n",
__func__, __LINE__, getRollGyro(), getPitchGyro(),
getYawGyro());
_DEBUG(DEBUG_ACC, "(%s-%d) ACC: x=%3.3f y=%3.3f z=%3.3f\n",
__func__, __LINE__, getXAcc(), getYAcc(), getZAcc());
}
task main()
{
float leftFront, leftBack, rightFront, rightBack; // motors
float y, x, c;
tPSP controller;
/***** BEGIN Mecanum Field Oriented Drive Test *****/
init();
wait10Msec(100);
StartTask(readSensors, 8);
StartTask(displaySmartDiags, 255);
//if (bCompetitionMode) {waitForStart();}
while (true)
{
nxtDisplayCenteredTextLine(3, "%i", gyro_getheading());
/***** Proportional Motor Control *****/
PSPV4readButtons(PSPNXV4, controller);
//scale to -1 to 1
y = (deadband(k_deadband,-controller.joystickLeft_y))/100; //strafe
x = (deadband(k_deadband,controller.joystickLeft_x))/100; //forward/rev
c = (deadband(k_deadband,controller.joystickRight_x))/100; //spin
nxtDisplayTextLine(4, "%i", controller.joypadRight);
nxtDisplayTextLine(4, "%i", controller.joypadLeft);
if ((y == 0) && (x == 0))
{
x += controller.joypadRight; //strafe
x -= controller.joypadLeft; //strafe
y += controller.joypadUp; //forward/rev
y -= controller.joypadDown; //forward/rev
}
nxtDisplayTextLine(5, "%i", y);
mecanum_arcadeFOD(y, x, c, gyro_getheading(),
leftFront, rightFront, leftBack, rightBack);
motor[Lf] = leftFront*100;
motor[Rf] = rightFront*100;
motor[Lb] = leftBack*100;
motor[Rb] = rightBack*100;
if(controller.circleBtn == 1) { gyro_reset(); }
while(nNxtButtonPressed == kEnterButton) { gyro_reset(); }
wait1Msec(5);
}
}
//Called to run the robot.
task usercontrol()
{
startTask (Drive_control);
startTask (catapult);
startTask (liftPlatform);
while (true)
{
driveSpeed = deadband(vexRT[Ch3]);
turnCoef = deadband(vexRT[Ch1]);
wait1Msec(dt);
}
}
task main()
{
waitForStart(); // wait for start of tele-op phase
//initializeRobot();
while (true)
{
getJoystickSettings(joystick);
drive(deadband(joystick.joy1_x1), deadband(joystick.joy1_y1), deadband(joystick.joy1_x2)); // normal drive
liftTalons(joy1Btn(4));
dropTalons(joy1Btn(1));
}
}
void Turn(int power, long distance)
{
writeDebugStreamLine("new *****");
//writeDebugStreamLine("value: %d", SensorValue[Gyro]);
//int zero = initialize() + 59;//59 is added to perfectly 0 it.
long sum = 0;
//writeDebugStreamLine("delta: %d", SensorValue[Gyro] - zero);// begining delta is 59 +-1 often
//writeDebugStreamLine("zero: %d", zero);
//writeDebugStreamLine("value: %d", SensorValue[Gyro]);
//nMotorEncoder[motorB] = 0;
motor[motorD] = power;
motor[motorE] = -1 * power;
while( abs(sum) < abs(distance))// +-8400 is 90 degrees
{
//writeDebugStreamLine("%d", SensorValue[Gyro] - zero);
sum = sum + deadband(HTGYROreadRot(HTGYRO), 5);
wait1Msec(10);// update interval
writeDebugStreamLine("sum: %d", sum);
writeDebugStreamLine("Gyro: %4d", HTGYROreadRot(HTGYRO));
nxtDisplayTextLine(4, "Gyro: %4d", HTGYROreadRot(HTGYRO));
}
//writeDebugStreamLine("sum: %d", sum);
motor[motorD] = 0;
motor[motorE] = 0;
wait10Msec(1); // just to let the encoders reset when the robot is still
nMotorEncoder[motorE] = 0;
nMotorEncoder[motorD] = 0;
}
// Called repeatedly when this Command is scheduled to run
void DriveBot::Execute() {
Joystick* stick = Robot::oi->getGamePad();
if(Robot::mover->speedSwitch->Get()){
Robot::mover->rightMotor0->Set(deadband(stick->GetThrottle()*-0.75,.01));
Robot::mover->rightMotor1->Set(deadband(stick->GetThrottle()*-0.75,.01));
Robot::mover->leftMotor2->Set(deadband(stick->GetY()*0.75,.01));
Robot::mover->leftMotor3->Set(deadband(stick->GetY()*0.75,.01));
}else{
Robot::mover->rightMotor0->Set(stick->GetThrottle()*-0.25);
Robot::mover->rightMotor1->Set(stick->GetThrottle()*-0.25);
Robot::mover->leftMotor2->Set(stick->GetY()*0.25);
Robot::mover->leftMotor3->Set(stick->GetY()*0.25);
}
}
void Chassis::DriveWithJoysticks(){
float left = deadband(Robot::oi->getleftJoy()->GetY(),0.05);
/*if (photonOn) {
photonCannon->Set(Relay::kForward);
} else {
photonCannon->Set(Relay::kReverse);
}
*/
if (Robot::pneumaticSub->GetGShiftSolenoid()->Get()){
rightMotor1->ConfigNeutralMode(CANSpeedController::NeutralMode::kNeutralMode_Brake);
rightMotor2->ConfigNeutralMode(CANSpeedController::NeutralMode::kNeutralMode_Brake);
float right = deadband(-1*Robot::oi->getGamepad()->GetThrottle(),0.05);
leftMotor1->Set(left);
leftMotor2->Set(left);
if (right>0) {
if (climbLS->Get()) {
rightMotor1->Set(right*0.7);
rightMotor2->Set(right*0.7);
} else {
rightMotor1->Set(0);
rightMotor2->Set(0);
}
} else {
rightMotor1->Set(right);
rightMotor2->Set(right);
}
} else {
rightMotor1->ConfigNeutralMode(CANSpeedController::NeutralMode::kNeutralMode_Coast);
rightMotor2->ConfigNeutralMode(CANSpeedController::NeutralMode::kNeutralMode_Coast);
float right = deadband(Robot::oi->getrightJoy()->GetY(),0.05);
if (Robot::oi->getrightJoy()->GetRawButton(1) && Robot::oi->getleftJoy()->GetRawButton(1)){
left = left * -0.5;
right = right * -0.5;
driveMotors->TankDrive(right,left);
} else {
driveMotors->TankDrive(left,right);
}
}
}
//
// main() is where program execution begins
//
task main()
{
waitForStart(); // wait for start of tele-op phase
while (true) // infinite loop
{
// Read the latest joystick data from the field/driverstation
getJoystickSettings(joystick);
// Read the left and right joystick y axis
int left = joystick.joy1_y1;
int right = joystick.joy1_y2;
// Apply deadband function
left = deadband(left, DRIVE_JOYSTICK_DB);
right = deadband(right, DRIVE_JOYSTICK_DB);
// Drive with the joysticks
motor[leftDrive] = left;
motor[rightDrive] = right;
}
}
void TankDrive::RevArcadeDrive(Joystick* stick)
{
float xAxis = deadband(-1*stick->GetX(), .2,0);
float yAxis = deadband(stick->GetY(), .2,0);
float zAxis = deadband(stick->GetZ(), .3,0);
float left=0;
float right=0;
if(!zAxis==0)
{
left=zAxis;
right=-1*zAxis;
}
else if(!xAxis==0)
{
if(xAxis<0)
{
left=(fabs(yAxis)-fabs(xAxis))*(yAxis/fabs(yAxis));
right=yAxis;
}
if(xAxis>0)
{
right=(fabs(yAxis)-fabs(xAxis))*(yAxis/fabs(yAxis));
left=yAxis;
}
}
else if(!yAxis==0)
{
left=yAxis;
right=yAxis;
}
leftTarget=left*maxTicks;
rightTarget=right*maxTicks;
}
// Task for the driver controlled portion of the competition.
task usercontrol()
{
startTask(Pid1);
startTask(Pid2);
startTask(Drive_control);
while (true)
{
// Drive control
driveSpeed = deadband(vexRT[Ch3]);
turnCoef = deadband(vexRT[Ch1]);
/* if (vexRT(Btn7L) == 1) {
if (victory == 0) {
startTask(victoryDance);
victory = 1;
}
} else {
victory = 0;
} */
sleep(20);
// Intake control
upperSpeed = 0;
lowerSpeed = 0;
gateSpeed = 0;
if (vexRT[Btn6U] == 1) { // run both intake motors up when button 6 up pressed
lowerSpeed = 127;
if (launcherSpeed == maxLauncherSpeed) {
upperSpeed = 127;
}
} else if (vexRT[Btn6D] == 1){ // both intake motors up when button 6D pressed
lowerSpeed = -127;
upperSpeed = -127;
}
if (vexRT[Btn8UXmtr2] == 1){
gateSpeed = 127;
}
if (vexRT[Btn8DXmtr2] == 1){
gateSpeed = -127;
}
motor[gate] = gateSpeed;
// Individual intake control
if (vexRT[Btn5U] == 1) {
upperSpeed = 127; // upper intake runs up
} else if (vexRT[Btn5D] == 1) {
lowerSpeed = 127; // lower intake runs up
}
motor[intakeLower] = lowerSpeed;
motor[intakeUpper] = upperSpeed;
motor[cuteIntake] = lowerSpeed;
// Launcher
if (vexRT[Btn6UXmtr2] == 1 && launcherSpeed < maxLauncherSpeed) {
launcherSpeed++; // add 1 to the launcher speed
} else if (launcherSpeed > 0 && vexRT[Btn6UXmtr2] == 0) {
launcherSpeed--; // subtract 1 to launcher speed
}
//motor[leftLauncher] = launcherSpeed;
//motor[rightLauncher] = launcherSpeed;
// now done by pid loops
}
}
static void stabilizerAltHoldUpdate(void)
{
// Get altitude hold commands from pilot
commanderGetAltHold(&altHold, &setAltHold, &altHoldChange);
// Get barometer height estimates
//TODO do the smoothing within getData
ms5611GetData(&pressure, &temperature, &aslRaw);
asl = asl * aslAlpha + aslRaw * (1 - aslAlpha);
aslLong = aslLong * aslAlphaLong + aslRaw * (1 - aslAlphaLong);
// Estimate vertical speed based on successive barometer readings. This is ugly :)
vSpeedASL = deadband(asl - aslLong, vSpeedASLDeadband);
// Estimate vertical speed based on Acc - fused with baro to reduce drift
vSpeed = constrain(vSpeed, -vSpeedLimit, vSpeedLimit);
vSpeed = vSpeed * vBiasAlpha + vSpeedASL * (1.f - vBiasAlpha);
vSpeedAcc = vSpeed;
// Reset Integral gain of PID controller if being charged
if (!pmIsDischarging())
{
altHoldPID.integ = 0.0;
}
// Altitude hold mode just activated, set target altitude as current altitude. Reuse previous integral term as a starting point
if (setAltHold)
{
// Set to current altitude
altHoldTarget = asl;
// Cache last integral term for reuse after pid init
const float pre_integral = altHoldPID.integ;
// Reset PID controller
pidInit(&altHoldPID, asl, altHoldKp, altHoldKi, altHoldKd,
ALTHOLD_UPDATE_DT);
// TODO set low and high limits depending on voltage
// TODO for now just use previous I value and manually set limits for whole voltage range
// pidSetIntegralLimit(&altHoldPID, 12345);
// pidSetIntegralLimitLow(&altHoldPID, 12345); /
altHoldPID.integ = pre_integral;
// Reset altHoldPID
altHoldPIDVal = pidUpdate(&altHoldPID, asl, false);
}
// In altitude hold mode
if (altHold)
{
// Update target altitude from joy controller input
altHoldTarget += altHoldChange / altHoldChange_SENS;
pidSetDesired(&altHoldPID, altHoldTarget);
// Compute error (current - target), limit the error
altHoldErr = constrain(deadband(asl - altHoldTarget, errDeadband),
-altHoldErrMax, altHoldErrMax);
pidSetError(&altHoldPID, -altHoldErr);
// Get control from PID controller, dont update the error (done above)
// Smooth it and include barometer vspeed
// TODO same as smoothing the error??
altHoldPIDVal = (pidAlpha) * altHoldPIDVal + (1.f - pidAlpha) * ((vSpeedAcc * vSpeedAccFac) +
(vSpeedASL * vSpeedASLFac) + pidUpdate(&altHoldPID, asl, false));
// compute new thrust
actuatorThrust = max(altHoldMinThrust, min(altHoldMaxThrust,
limitThrust( altHoldBaseThrust + (int32_t)(altHoldPIDVal*pidAslFac))));
// i part should compensate for voltage drop
}
else
{
altHoldTarget = 0.0;
altHoldErr = 0.0;
altHoldPIDVal = 0.0;
}
}
float SpektrumRX::getThrottle(void){
return deadband(reference_command[0], 0.2);
}
float SpektrumRX::getRoll(void){
return deadband(reference_command[1], 0.05);
}
float SpektrumRX::getPitch(void){
return deadband(reference_command[2], 0.05);
}
float SpektrumRX::getYaw(void){
return deadband(reference_command[3], 0.05);
}
static void stabilizerTask(void* param) {
uint32_t attitudeCounter = 0;
uint32_t altHoldCounter = 0;
uint32_t lastWakeTime;
vTaskSetApplicationTaskTag(0, (void*) TASK_STABILIZER_ID_NBR);
//Wait for the system to be fully started to start stabilization loop
systemWaitStart();
lastWakeTime = xTaskGetTickCount();
while (1) {
vTaskDelayUntil(&lastWakeTime, F2T(IMU_UPDATE_FREQ)); // 500Hz
// Magnetometer not yet used more then for logging.
imu9Read(&gyro, &acc, &mag);
if (imu6IsCalibrated()) {
commanderGetRPY(&eulerRollDesired, &eulerPitchDesired, &eulerYawDesired);
commanderGetRPYType(&rollType, &pitchType, &yawType);
// 250HZ
if (++attitudeCounter >= ATTITUDE_UPDATE_RATE_DIVIDER) {
sensfusion6UpdateQ(gyro.x, gyro.y, gyro.z, acc.x, acc.y, acc.z, FUSION_UPDATE_DT);
sensfusion6GetEulerRPY(&eulerRollActual, &eulerPitchActual, &eulerYawActual);
accWZ = sensfusion6GetAccZWithoutGravity(acc.x, acc.y, acc.z);
accMAG = (acc.x * acc.x) + (acc.y * acc.y) + (acc.z * acc.z);
// Estimate speed from acc (drifts)
vSpeed += deadband(accWZ, vAccDeadband) * FUSION_UPDATE_DT;
controllerCorrectAttitudePID(eulerRollActual, eulerPitchActual, eulerYawActual,
eulerRollDesired, eulerPitchDesired, -eulerYawDesired, &rollRateDesired,
&pitchRateDesired, &yawRateDesired);
attitudeCounter = 0;
}
// 100HZ
if (imuHasBarometer() && (++altHoldCounter >= ALTHOLD_UPDATE_RATE_DIVIDER)) {
stabilizerAltHoldUpdate();
altHoldCounter = 0;
}
if (rollType == RATE) {
rollRateDesired = eulerRollDesired;
}
if (pitchType == RATE) {
pitchRateDesired = eulerPitchDesired;
}
if (yawType == RATE) {
yawRateDesired = -eulerYawDesired;
}
// TODO: Investigate possibility to subtract gyro drift.
controllerCorrectRatePID(gyro.x, -gyro.y, gyro.z, rollRateDesired, pitchRateDesired,
yawRateDesired);
controllerGetActuatorOutput(&actuatorRoll, &actuatorPitch, &actuatorYaw);
if (!altHold || !imuHasBarometer()) {
// Use thrust from controller if not in altitude hold mode
commanderGetThrust(&actuatorThrust);
} else {
// Added so thrust can be set to 0 while in altitude hold mode after disconnect
commanderWatchdog();
}
if (actuatorThrust > 0) {
#if defined(TUNE_ROLL)
distributePower(actuatorThrust, actuatorRoll, 0, 0);
#elif defined(TUNE_PITCH)
distributePower(actuatorThrust, 0, actuatorPitch, 0);
#elif defined(TUNE_YAW)
distributePower(actuatorThrust, 0, 0, -actuatorYaw);
#else
distributePower(actuatorThrust, actuatorRoll, actuatorPitch, -actuatorYaw);
#endif
} else {
distributePower(0, 0, 0, 0);
controllerResetAllPID();
}
}
}
}
task main()
{
waitForStart(); // wait for start of tele-op phase
// initializeRobot();
while (true)
{
getJoystickSettings(joystick); // update joystick input
drive(deadband(joystick.joy1_x1), deadband(joystick.joy1_y1), deadband(joystick.joy1_x2)); // normal drive
manualLiftSlide(deadband(joystick.joy2_y1), deadband(joystick.joy2_y2));
if (joy1Btn(4) == 1) {
liftTalons();
}
if (joy1Btn(1) == 1) {
dropTalons();
}
if (joy2Btn(3) == 1)
liftHood();
if (joy2Btn(2) == 1)
dropHood();
if (joy2Btn(6) == 1)
openGate();
if (joy2Btn(5) == 1)
closeGate();
//if (joy2Btn(8))
//{
// while (joy2Btn(8)) // Wait while pressed
// {
// getJoystickSettings(joystick);
// }
// if (brushToggle)
// brushToggle = false;
// else
// brushToggle = true;
//}
//if (brushToggle)
// motor[Brush] = 90;
//else
// motor[Brush] = 0;
if (joy1Btn(5))
motor[Brush] = 75;
if (joy1Btn(6))
motor[Brush] = 0;
if (joy2Btn(1))
motor[Shooter] = -90;
else
motor[Shooter] = 0;
// ball shooting
//if (ServoValue[])
}
}
static void stabilizerAltHoldUpdate() {
// Get the time
float altitudeError = 0;
static float altitudeError_i = 0;
float instAcceleration = 0;
float deltaVertSpeed = 0;
static uint32_t timeStart = 0;
static uint32_t timeCurrent = 0;
// Get altitude hold commands from pilot
commanderGetAltHold(&altHold, &setAltHold, &altHoldChange);
// Get barometer height estimates
//TODO do the smoothing within getData
ms5611GetData(&pressure, &temperature, &aslRaw);
// Compute the altitude
altitudeError = aslRaw - estimatedAltitude;
altitudeError_i = fmin(50.0, fmax(-50.0, altitudeError_i + altitudeError));
instAcceleration = deadband(accWZ, vAccDeadband) * 9.80665 + altitudeError_i * altEstKi;
deltaVertSpeed = instAcceleration * ALTHOLD_UPDATE_DT + (altEstKp1 * ALTHOLD_UPDATE_DT) * altitudeError;
estimatedAltitude += (vSpeedComp * 2.0 + deltaVertSpeed) * (ALTHOLD_UPDATE_DT / 2) + (altEstKp2 * ALTHOLD_UPDATE_DT) * altitudeError;
vSpeedComp += deltaVertSpeed;
aslLong = estimatedAltitude; // Override aslLong
// Estimate vertical speed based on Acc - fused with baro to reduce drift
vSpeedComp = constrain(vSpeedComp, -vSpeedLimit, vSpeedLimit);
// Reset Integral gain of PID controller if being charged
if (!pmIsDischarging()) {
altHoldPID.integ = 0.0;
}
// Altitude hold mode just activated, set target altitude as current altitude. Reuse previous integral term as a starting point
if (setAltHold) {
// Set to current altitude
altHoldTarget = estimatedAltitude;
// Set the start time
timeStart = xTaskGetTickCount();
timeCurrent = 0;
// Reset PID controller
pidInit(&altHoldPID, estimatedAltitude, altHoldKp, altHoldKi, altHoldKd, ALTHOLD_UPDATE_DT);
// Cache last integral term for reuse after pid init
// const float pre_integral = hoverPID.integ;
// Reset the PID controller for the hover controller. We want zero vertical velocity
pidInit(&hoverPID, 0, hoverKp, hoverKi, hoverKd, ALTHOLD_UPDATE_DT);
pidSetIntegralLimit(&hoverPID, 3);
// TODO set low and high limits depending on voltage
// TODO for now just use previous I value and manually set limits for whole voltage range
// pidSetIntegralLimit(&altHoldPID, 12345);
// pidSetIntegralLimitLow(&altHoldPID, 12345); /
// hoverPID.integ = pre_integral;
}
// In altitude hold mode
if (altHold) {
// Get current time
timeCurrent = xTaskGetTickCount();
// Update target altitude from joy controller input
altHoldTarget += altHoldChange / altHoldChange_SENS;
pidSetDesired(&altHoldPID, altHoldTarget);
// Compute error (current - target), limit the error
altHoldErr = constrain(deadband(estimatedAltitude - altHoldTarget, errDeadband), -altHoldErrMax, altHoldErrMax);
pidSetError(&altHoldPID, -altHoldErr);
// Get control from PID controller, dont update the error (done above)
float altHoldPIDVal = pidUpdate(&altHoldPID, estimatedAltitude, false);
// Get the PID value for the hover
float hoverPIDVal = pidUpdate(&hoverPID, vSpeedComp, true);
float thrustValFloat;
// Use different weights depending on time into altHold mode
if (timeCurrent > 150) {
// Compute the mixture between the alt hold and the hover PID
thrustValFloat = 0.5*hoverPIDVal + 0.5*altHoldPIDVal;
} else {
// Compute the mixture between the alt hold and the hover PID
thrustValFloat = 0.1*hoverPIDVal + 0.9*altHoldPIDVal;
}
// float thrustVal = 0.5*hoverPIDVal + 0.5*altHoldPIDVal;
uint32_t thrustVal = altHoldBaseThrust + (int32_t)(thrustValFloat*pidAslFac);
// compute new thrust
actuatorThrust = max(altHoldMinThrust, min(altHoldMaxThrust, limitThrust( thrustVal )));
// i part should compensate for voltage drop
} else {
altHoldTarget = 0.0;
altHoldErr = 0.0;
}
}
/*Control Layout:
Controller 1:
Left Joystick x/y - Strafe and forward for robot
Right Joystick x - Turn
Button 1: Goal Latch Closed
Button 3: Goal Latch Open
Button 4: Gyro Reset
Button 8: Slo-Mo
Controller 2:
Button 1: Blower
Button 2: Intake
Button 3: Ball storage
Button 4: Kickstand
Button 5: Intake slow (lift release)
Button 6: Intake backwards
Button 7: Touch sensor
Timers:
T1:Gyro
T2:Measure RPM
T3:Blower
T4:Global
*/
task main()
{
float leftFront, leftBack, rightFront, rightBack; // motors
float y, x, c;
bool touchsensorenabled = false;
bool blowerenabled = false;
bool kickstandenabled = false;
bool storageclosed = false;
bool intakeenabled = false;
bool estop = false;
bool intakestartup = false;
long intakelastchecked = -10000;
int joy1Btn3last = 0;
int joy1Btn2last = 0;
int joy1Btn1last = 0;
//Operator
int joy2Btn1last = 0;
int joy2Btn2last = 0;
int joy2Btn3last = 0;
int joy2Btn4last = 0;
int power = 100; //power for drive motors
/***** BEGIN Mecanum Field Oriented Drive Test *****/
init();
StartTask(readSensors);
StartTask(displaySmartDiags);
if (bCompetitionMode) {waitForStart();}
ClearTimer(T4);
tele_log_init();
while (true)
{
/***** Proportional Motor Control *****/
getJoystickSettings(joystick); //get all joystick statuses
if (joy1Btn(8))
{
power = 25;
}
else { power = 100; }
//Drive Code
if ((deadband(k_deadband,joystick.joy1_y1) == 0 &&
deadband(k_deadband,joystick.joy1_x1) == 0 &&
deadband(k_deadband,joystick.joy1_x2) == 0)) {
motor[Lf] = 0;
motor[Rf] = 0;
motor[Lb] = 0;
motor[Rb] = 0;
}
else {
//scale to -1 to 1
y = ((deadband(k_deadband,joystick.joy1_y1)+1)/128); //strafe
x = ((deadband(k_deadband,joystick.joy1_x1)+1)/128); //forward/rev
c = ((deadband(k_deadband,joystick.joy1_x2)+1)/128); //spin
mecanum_arcadeFOD(y, x, c, gyro_getheading(),
leftFront, rightFront, leftBack, rightBack);
motor[Lf] = leftFront*power;
motor[Rf] = rightFront*power;
motor[Lb] = leftBack*power;
motor[Rb] = rightBack*power;
}
//Gyro Reset Code
if(joy1Btn(4) == 1) { gyro_reset(); }
if(joystick.joy1_TopHat == 0) {
gyro_reset();
}
//90 Deg
if(joystick.joy1_TopHat == 2) {
gyro_reset();
gyro_set(90);
}
//180 Deg
if(joystick.joy1_TopHat == 4) {
gyro_reset();
gyro_set(180);
}
//270 Deg
if(joystick.joy1_TopHat == 6) {
gyro_reset();
gyro_set(270);
}
if(nNxtButtonPressed == kEnterButton) { gyro_reset(); }
if (joy1Btn(7) == 1){
servo[GoalRetainer] = 255;
}
//Goal Latch Open
if(joy1Btn(3)== 1 && joy1Btn3last != 1){
servo[GoalRetainer] = 130;
}
//Goal Latch Closed
if(joy1Btn(1)== 1 && joy1Btn1last != 1){
servo[GoalRetainer] = 5;
}
//Blower Toggle
if(joy2Btn(1)== 1 && joy2Btn1last != 1){
if (blowerenabled){
motor[BlowerA] = 1;
motor[BlowerB] = 1;
motor[BlowerC] = 1;
//Start timer for 1 sec, then set motors to 0
ClearTimer(T3);
blowerenabled = false;
log_write("BL","OFF");
}
else{
ClearTimer(T2);
nMotorEncoder[BlowerB] = 0;
motor[BlowerA] = 100;
motor[BlowerB] = 100;
motor[BlowerC] = 100;
blowerenabled = true;
log_write("BL","ON");
}
}
//If 5 seconds since blower shutdown-brake motors
if (time1[T3] > 5000 && !blowerenabled){
motor[BlowerA] = 0;
motor[BlowerB] = 0;
motor[BlowerC] = 0;
}
//Intake Forwards, back, slow forward
if (joy2Btn(2) && joy2Btn2last != 1){
if (intakeenabled){
intakeenabled = false;
log_write("IN","OFF");
}
else{
intakelastchecked = -10000;
nMotorEncoder[Rf] = 4000;
intakeenabled = true;
intakestartup = true;
log_write("IN","ON");
}
}
int nIntakeHealthyVal = 400;
if (joy2Btn(5) == 1){
motor[Intake] = 25;
}
else if (joy2Btn(6) == 1){
motor[Intake] = -100;
}
else if (joy2Btn(7) == 1){
motor[Intake] = 100;
}
else if (joy2Btn(8) == 1){
motor[Intake] = -50;
}
else if (!intakeenabled){
motor[Intake] = 0;
}
else if (intakeenabled && time1[T4] > intakelastchecked+500){
if (abs(nMotorEncoder[Rf]) > nIntakeHealthyVal || intakestartup){
motor[Intake] = 100;
intakestartup = false;
}
else {
motor[Intake] = -100;
}
nMotorEncoder[Rf] = 0;
intakelastchecked = time1[T4];
}
//Storage Toggle
if(joy2Btn(3)== 1 && joy2Btn3last != 1){
if (storageclosed){
servo[BallStorage] = 80;
storageclosed = false;
}
else{
servo[BallStorage] = 140;
storageclosed = true;
}
}
//Kickstand Toggle
if(joy2Btn(4)== 1 && joy2Btn4last != 1){
if (kickstandenabled){
servo[Kickstand] = 155;
kickstandenabled = false;
}
else{
servo[Kickstand] = 31;
kickstandenabled = true;
}
}
//Touch Sensor Toggle
if(joy1Btn(2)== 1 && joy1Btn2last != 1){
if (touchsensorenabled){
servo[TouchSensor] = 65;
touchsensorenabled = false;
}
else{
servo[TouchSensor] = 190;
touchsensorenabled = true;
}
}
//E Stop for blower
if (!estop && time1[T4] > 117000 && blowerenabled){
motor[BlowerA] = 1;
motor[BlowerB] = 1;
motor[BlowerC] = 1;
//Start timer for 1 sec, then set motors to 0
ClearTimer(T3);
blowerenabled = false;
estop = true;
}
else if (time1[T4] >117000){
log_stop();
}
//VOLTAGE
if (time100[T2] >= 20) {
float v = (float)externalBatteryAvg / (float)1000;
string sv = "";
StringFormat(sv, "%1.2f", v);
log_write("V", sv);
if (blowerenabled) {
float dEncoderCount = nMotorEncoder[BlowerB];
float dTime = time1[T2];
float rpm = (4439 * (dEncoderCount))/(dTime);
string s = "";
StringFormat(s,"RPM: %1.2f",rpm);
nxtDisplayTextLine(5, s);
log_write("BL",s);
nMotorEncoder[BlowerB] = 0;
}
//Clear for next calculation
ClearTimer(T2);
}
//Winch
if (deadband(k_deadband,joystick.joy2_y2) == 0){
servo[TubeWinch] = 127;
}
else{
servo[TubeWinch] = (deadband(k_deadband,joystick.joy2_y2))+127;
}
joy1Btn1last = joy1Btn(1);
joy1Btn2last = joy1Btn(2);
joy1Btn3last = joy1Btn(3);
joy2Btn1last = joy2Btn(1);
joy2Btn2last = joy2Btn(2);
joy2Btn3last = joy2Btn(3);
joy2Btn4last = joy2Btn(4);
//DO NOT REMOVE THIS WAIT, See issue #11
nxtDisplayTextLine(4, "%i", gyro_getheading());
wait1Msec(5);
}
}
