/*
* main
* main function of the robot
*/
task main(){
initFlags();
initClamp();
initHitch();
startTask(task_run_drivemode);
while(true){
//Check if we should drive with slow sleeds.
slow_multiplier = 1.0 - (vexRT[JOY_BTN_SLOW] * SLOW_SPEED_MULTIPLIER);
//Check if we should switch the drivemode
checkSwitchDrivemode();
checkSwitchFlagMode();
//Call the Arm control method
runArm();
//Call the Claw control method
runClamp();
//Call the Hitch control method
runHitch();
//Call the task handling method
checkRunModeTask();
}
}
// main program
int main(void) {
// setup DAQ
if (setupDAQ(DAQ_CHANNEL)) {
runArm();
}
else {
return 1;
}
}
void cliFuncArm(void *cmd, char *cmdLine) {
if (state > ESC_STATE_DISARMED) {
serialPrint("ESC already armed\r\n");
}
else {
if (runMode != SERVO_MODE)
cliFuncChangeInput(ESC_INPUT_UART);
runArm();
serialPrint("ESC armed\r\n");
}
}
task ScoringArm()
{
while(true)
{
//-----------joystick 2------------//
//high priority
getJoystickSettings(joystick);
if(joy2Btn(6) && !joy2Btn(8)) //move arm up
{
runArm(80);
}
else if(joy2Btn(8) && !joy2Btn(6)) //move arm down
{
runArm(-40);
}
else //if not pressed/pressed at same time
{
joy1Arm();
}
}
}
void joy1Arm()
{
while(true)
{
//-----------joystick 1------------//
//low priority
getJoystickSettings(joystick);
if(joy1Btn(6) && !joy1Btn(8)) //move arm up
{
runArm(80);
}
else if(joy1Btn(8) && !joy1Btn(6)) //move arm down
{
runArm(-40);
}
else //if not pressed/pressed at same time
{
runArm(STOP);
}
}
}
void runWatchDog(void) {
register uint32_t t, d, p;
__asm volatile ("cpsid i");
t = timerMicros;
d = detectedCrossing;
p = pwmValidMicros;
__asm volatile ("cpsie i");
if (state == ESC_STATE_STARTING && fetGoodDetects > fetStartDetects) {
state = ESC_STATE_RUNNING;
digitalHi(statusLed); // turn off
}
else if (state >= ESC_STATE_STOPPED) { // running or starting
d = (t >= d) ? (t - d) : (TIMER_MASK - d + t);
// timeout if PWM signal disappears
if (inputMode == ESC_INPUT_PWM) {
p = (t >= p) ? (t - p) : (TIMER_MASK - p + t);
if (p > PWM_TIMEOUT)
runDisarm(REASON_PWM_TIMEOUT);
}
// if (state == ESC_STATE_RUNNING && d > ADC_CROSSING_TIMEOUT) {
if (state >= ESC_STATE_STARTING && d > ADC_CROSSING_TIMEOUT) {
if (fetDutyCycle > 0) {
runDisarm(REASON_CROSSING_TIMEOUT);
}
else {
runArm();
pwmIsrRunOn();
}
}
else if (state >= ESC_STATE_STARTING && fetBadDetects > fetDisarmDetects) {
if (fetDutyCycle > 0)
runDisarm(REASON_BAD_DETECTS);
}
else if (state == ESC_STATE_STOPPED) {
adcAmpsOffset = adcAvgAmps; // record current amperage offset
}
}
else if (state == ESC_STATE_DISARMED && !(runMilis % 100)) {
adcAmpsOffset = adcAvgAmps; // record current amperage offset
digitalTogg(errorLed);
}
}
/*
* main
* main function of the robot
*/
task main(){
while(true){
//Check if we should drive with slow sleeds.
slow = 1.0 - (vexRT[JOY_BTN_SLOW] * SLOW_SPEED_MULTIPLIER);
//Call to the drive methods
tankDrive();
moveBtns();
//Call the Arm control method
runArm();
//Call the Calw control method
runClaw();
//If we are pushing our special button
//Set the arm motor to move to the pre-defined position
if(vexRT[JOY_BTN_MOVE_POINT]) startTask(task_move_to_height, kDefaultTaskPriority);
}
}
void runNewInput(uint16_t setpoint) {
static uint16_t lastPwm;
static float filteredSetpoint = 0;
// Lowpass Input if configured
// TODO: Make lowpass independent from pwm update rate
if (p[PWM_LOWPASS]) {
filteredSetpoint = (p[PWM_LOWPASS] * filteredSetpoint + (float)setpoint) / (1.0f + p[PWM_LOWPASS]);
setpoint = filteredSetpoint;
}
if (state == ESC_STATE_RUNNING && setpoint != lastPwm) {
if (runMode == OPEN_LOOP) {
fetSetDutyCycle(fetPeriod * (int32_t)(setpoint-pwmLoValue) / (int32_t)(pwmHiValue - pwmLoValue));
}
else if (runMode == CLOSED_LOOP_RPM) {
float target = p[PWM_RPM_SCALE] * (setpoint-pwmLoValue) / (pwmHiValue - pwmLoValue);
// limit to configured maximum
targetRpm = (target > p[PWM_RPM_SCALE]) ? p[PWM_RPM_SCALE] : target;
}
// THRUST Mode
else if (runMode == CLOSED_LOOP_THRUST) {
float targetThrust; // desired trust
float target; // target(rpm)
// Calculate targetThrust based on input and MAX_THRUST
targetThrust = maxThrust * (setpoint-pwmLoValue) / (pwmHiValue - pwmLoValue);
// Workaraound: Negative targetThrust will screw up sqrtf() and create MAX_RPM on throttle min. Dangerous!
if (targetThrust > 0.0f) {
// Calculate target(rpm) based on targetThrust
target = ((sqrtf(p[THR1TERM] * p[THR1TERM] + 4.0f * p[THR2TERM] * targetThrust) - p[THR1TERM] ) / ( 2.0f * p[THR2TERM] ));
}
// targetThrust is negative (pwm_in < pwmLoValue)
else {
target = 0.0f;
}
// upper limit for targetRpm is configured maximum PWM_RPM_SCALE (which is MAX_RPM)
targetRpm = (target > p[PWM_RPM_SCALE]) ? p[PWM_RPM_SCALE] : target;
}
lastPwm = setpoint;
}
else if ((state == ESC_STATE_NOCOMM || state == ESC_STATE_STARTING) && setpoint <= pwmLoValue) {
fetSetDutyCycle(0);
state = ESC_STATE_RUNNING;
}
else if (state == ESC_STATE_DISARMED && setpoint > pwmMinValue && setpoint <= pwmLoValue) {
runArmCount++;
if (runArmCount > RUN_ARM_COUNT)
runArm();
}
else {
runArmCount = 0;
}
if (state == ESC_STATE_STOPPED && setpoint >= pwmMinStart) {
runStart();
}
}
