int main() {
pc.printf("Starting up\r\n");
timer.start();
freqReader.start();
loadMotor.period(0.01f); // 100 Hz
loadMotor.write(0.1f); // Initially 50% duty cycle
max = 0.0f;
min = 1.0f;
count = 0;
loadDuty = 0.2f;
loadDirection = 1;
while(1) {
float yellow = loadEncoderYellow.read();
float green = loadEncoderGreen.read();
float net = green - yellow;
pc.printf("Net Output: %f\r\n", net);
/* Maximum and minimum value calculations for testing */
if(net > max) {
max = net;
}
if(net < min) {
min = net;
}
if(net > 0.5f) {
count++;
}
if(count > 25) {
risingEdge();
count = 0;
}
float currentTime = timer.read();
if(currentTime > 5) {
break;
loadDuty = loadDuty + 0.05f;
timer.reset();
}
if(loadDirectionLocal < 0) {
loadDirection = 0;
} else {
loadDirection = 1;
}
loadMotor.write(loadDuty);
}
}
task lcd() {
const float flyPwrIncrement = 5;
const word battThresh = 7800;
const long pwrBtnsDelayInterval = 750,
pwrBtnsRepeatInterval = 100,
dispPwrTimeout = 1000;
bool dispPwr = false,
doUseLRPwr = true,
flash = false,
forceBattWarning = true,
pwrBtnsDown,
pwrBtnsRepeating;
float pwrBtns;
long time = nSysTime, flashTs = time, dispPwrTs = time, pwrBtnTs = time;
string str;
DLatch dismissWarningLatch, pwrBtnLatch, pwrBtnLRModeLatch, pwrBtnSRModeLatch;
while (true) {
time = nSysTime;
if (nImmediateBatteryLevel < battThresh) {
if ((time - flashTs) >= 500) {
flash = !flash;
flashTs = time;
}
SensorValue[redLed] =
SensorValue[yellowLed] =
SensorValue[greenLed] = flash;
}
else {
SensorValue[redLed] =
SensorValue[yellowLed] =
SensorValue[greenLed] = 0;
}
clearLCD();
if (nLCDButtons & kButtonCenter) {
bLCDBacklight = true;
displayLCDCenteredString(0, "Battery:");
sprintBatt(str);
displayLCDString(1, 0, str);
}
else if (nImmediateBatteryLevel < battThresh && (rkBotDisabled || rkAutonMode || forceBattWarning)) {
bLCDBacklight = flash;
if (flash) displayLCDCenteredString(0, "BATTERY WARNING");
sprintBatt(str);
displayLCDString(1, 0, str);
if (fallingEdge(&dismissWarningLatch, nLCDButtons || (abs(vexRT[AccelY]) > 63))) forceBattWarning = false;
}
else if (rkBotDisabled) {
bLCDBacklight = false;
displayLCDCenteredString(0, "Moosebot Mk. II");
displayLCDCenteredString(1, "4800Buckets");
}
else if (rkAutonMode) {
bLCDBacklight = true;
displayLCDCenteredString(0, "BUCKETS MODE");
displayLCDCenteredString(1, "ENGAGED");
}
else { //User op mode
bLCDBacklight = true;
pwrBtnsDown = PWR_BTN_DOWN ^ PWR_BTN_UP;
risingEdge(&pwrBtnLatch, pwrBtnsDown);
if (pwrBtnsDown || (PWR_BTN_LRMODE ^ PWR_BTN_SRMODE)) dispPwrTs = time;
if (risingEdge(&pwrBtnLRModeLatch, PWR_BTN_LRMODE)) doUseLRPwr = true;
if (risingEdge(&pwrBtnSRModeLatch, PWR_BTN_SRMODE)) doUseLRPwr = false;
if (pwrBtnsDown) {
dispPwrTs = time;
if (pwrBtnLatch.out || (time - pwrBtnTs >= (pwrBtnsRepeating ? pwrBtnsRepeatInterval : pwrBtnsDelayInterval))) {
pwrBtnTs = time;
if (!pwrBtnsRepeating) pwrBtnsRepeating = true;
pwrBtns = twoWay((nLCDButtons & kButtonLeft) || vexRT[Btn7D], -flyPwrIncrement, (nLCDButtons & kButtonRight) || vexRT[Btn7U], flyPwrIncrement);
if (doUseLRPwr) flyLRPwr += pwrBtns;
else flySRPwr += pwrBtns;
}
}
if (time - dispPwrTs <= dispPwrTimeout) {
if (doUseLRPwr) {
displayLCDCenteredString(0, "Long Power:");
displayLCDNumber(1, 0, flyLRPwr);
}
else {
displayLCDCenteredString(0, "Short Power:");
displayLCDNumber(1, 0, flySRPwr);
}
}
else if (flyTbh.doRun) {
sprintf(str, "% 07.2f % 07.2f",
fmaxf(-999.99, fminf(999.99, flyDiff.out)),
fmaxf(-999.99, fminf(999.99, flyTbh.err)));
displayLCDString(0, 0, str);
sprintf(str, "% 07.2f % 07.2f",
fmaxf(-999.99, fminf(999.99, fly2Flt.out)),
fmaxf(-999.99, fminf(999.99, flyTbh.out)));
displayLCDString(1, 0, str);
SensorValue[redLed] =
SensorValue[yellowLed] =
SensorValue[greenLed] = 0;
if (isTbhInThresh(&flyTbh, velThresh)) {
if (isTbhDerivInThresh(&flyTbh, accelThresh)) SensorValue[greenLed] = 1;
else SensorValue[yellowLed] = 1;
}
else SensorValue[redLed] = 1;
}
else {
sprintf(str, "%-8s%8s", "Speed", "Error");
displayLCDString(0, 0, str);
sprintf(str, "%-8s%8s", "Accel", "Out");
displayLCDString(1, 0, str);
}
}
wait1Msec(50);
}
}
void TeleopJoy::joyCallback(const sensor_msgs::Joy::ConstPtr& joy) {
// init previous_buttons
if(previous_buttons.size() != joy->buttons.size()) {
previous_buttons = std::vector<int>(joy->buttons);
}
// arm, disarm
if(risingEdge(joy, config.disarm_button)) {
setArming(false);
}
else if(risingEdge(joy, config.arm_button)) {
setArming(true);
}
// mode switching
if(risingEdge(joy, config.stabilize_button)) {
mode = MODE_STABILIZE;
}
else if(risingEdge(joy, config.alt_hold_button)) {
mode = MODE_ALT_HOLD;
}
// takeoff and land
if(risingEdge(joy, config.land_button)) {
cmdTakeoffLand(false);
}
else if(risingEdge(joy, config.takeoff_button)) {
cmdTakeoffLand(true);
}
// change camera_tilt
if(risingEdge(joy, config.cam_tilt_reset)) {
camera_tilt = 1500;
}
else if(risingEdge(joy, config.cam_tilt_up)) {
camera_tilt = camera_tilt + config.cam_tilt_step;
if(camera_tilt > PPS_MAX) {
camera_tilt = PPS_MAX;
}
}
else if(risingEdge(joy, config.cam_tilt_down)) {
camera_tilt = camera_tilt - config.cam_tilt_step;
if(camera_tilt < PPS_MIN) {
camera_tilt = PPS_MIN;
}
}
// remember current button states for future comparison
previous_buttons = std::vector<int>(joy->buttons);
// send rc override message
mavros_msgs::OverrideRCIn msg;
msg.channels[5] = mapToPpm(config.x_scaling * computeAxisValue(joy, config.x_axis, config.expo)); // forward (x)
msg.channels[6] = mapToPpm(config.y_scaling * computeAxisValue(joy, config.y_axis, config.expo)); // strafe (y)
msg.channels[2] = mapToPpm(config.z_scaling * computeAxisValue(joy, config.z_axis, config.expo)); // throttle (z)
msg.channels[1] = mapToPpm(config.wx_scaling * computeAxisValue(joy, config.wx_axis, config.expo)); // roll (wx)
msg.channels[0] = mapToPpm(config.wy_scaling * computeAxisValue(joy, config.wy_axis, config.expo)); // pitch (wy)
msg.channels[3] = mapToPpm(config.wz_scaling * computeAxisValue(joy, config.wz_axis, config.expo)); // yaw (wz)
msg.channels[4] = mode; // mode
msg.channels[7] = camera_tilt; // camera tilt
rc_override_pub.publish(msg);
}
task userOp() {
startFlyTbh(true);
startCtlLoop();
const float cutFac = 3;
DLatch cutLatch, flipLatch, tankLatch, flyLRLatch, flySRLatch;
word driveA, driveB, flyDir = 0;
#define driveX driveA
#define driveY driveB
#define driveL driveA
#define driveR driveB
resetDLatch(&cutLatch, 0);
resetDLatch(&flipLatch, 0);
resetDLatch(&tankLatch, 0);
resetDLatch(&flyLRLatch, 0);
resetDLatch(&flySRLatch, 0);
while (true) {
risingBistable(&flipLatch, vexRT[Btn8D]);
if (risingBistable(&tankLatch, vexRT[Btn8L])) {
driveL = vexRT[ChLY];
driveR = vexRT[ChRY];
if (flipLatch.out) {
driveL ^= driveR;
driveR ^= driveL;
driveL ^= driveR;
driveL *= -1;
driveR *= -1;
}
}
else {
driveX = vexRT[ChRX];
driveY = vexRT[ChLY];
if (flipLatch.out) driveY *= -1;
}
if (risingBistable(&cutLatch, vexRT[Btn8U])) {
driveX = (word)(driveX / cutFac);
driveY = (word)(driveY / cutFac);
}
if (tankLatch.out) {
tank4(driveL, driveR, flWheel, blWheel, frWheel, brWheel);
}
else {
arcade4(driveX, driveY, flWheel, blWheel, frWheel, brWheel);
}
if (risingEdge(&flyLRLatch, vexRT[Btn5U]))
flyDir = flyDir == 1 ? 0 : 1;
if (risingEdge(&flySRLatch, vexRT[Btn5D]))
flyDir = flyDir == 2 ? 0 : 2;
switch (flyDir) {
case 1: updateTbhController(&flyCtl, flyLRPwr); break;
case 2: updateTbhController(&flyCtl, flySRPwr); break;
default: updateTbhController(&flyCtl, 0); break;
}
motor[intake] = motor[lift] =
joyDigi2(Btn6U, 127, Btn6D, -127);
wait1Msec(25);
}
#undef driveX
#undef driveY
#undef driveL
#undef driveR
}