View unanswered posts | View active topics It is currently Thu Nov 14, 2019 12:10 am

Reply to topic  [ 3 posts ] 
Autonomous Does not Function Properly 
Author Message

Joined: Mon Apr 13, 2015 9:58 pm
Posts: 3
Post Autonomous Does not Function Properly
I am fairly new to RobotC and am taking over programming for Team 4800 at Worlds. When I try to run our autonomous, the arm goes up and comes down, as it should, but then the robot stops. This happens every other time I try to run the program. Does anyone know why this happens?

Mon Apr 13, 2015 10:06 pm

Joined: Mon Apr 13, 2015 9:58 pm
Posts: 3
Post Re: Autonomous Does not Function Properly
Here is the code. I have no idea why our primary programmer used a custom competition template, or other ways of writing the program.
#pragma config(UART_Usage, UART1, uartVEXLCD, baudRate19200, IOPins, None, None)
#pragma config(UART_Usage, UART2, uartNotUsed, baudRate4800, IOPins, None, None)
#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, dgtl1,  armLim,         sensorTouch)
#pragma config(Sensor, I2C_1,  armEnc,         sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_2,  lDriveEnc,      sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_3,  rDriveEnc,      sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_4,  intakeEnc,      sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Motor,  port1,           blWheel,       tmotorVex393TurboSpeed_HBridge, openLoop, driveRight, encoderPort, I2C_2)
#pragma config(Motor,  port2,           frWheel,       tmotorVex393TurboSpeed_MC29, openLoop, reversed, driveLeft)
#pragma config(Motor,  port3,           blArm,         tmotorVex393_MC29, openLoop, encoderPort, I2C_1)
#pragma config(Motor,  port4,           tlArm,         tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port5,           rIntake,       tmotorVex393_MC29, openLoop, encoderPort, I2C_4)
#pragma config(Motor,  port6,           lIntake,       tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port7,           brArm,         tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port8,           trArm,         tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port9,           brWheel,       tmotorVex393_MC29, openLoop, reversed, driveLeft, encoderPort, I2C_3)
#pragma config(Motor,  port10,          flWheel,       tmotorVex393_HBridge, openLoop, driveRight)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "rkUtil\rkUtil001.h"
#include "rkPid\rkPid002.h"
#include "rkCompetition\rkCompetition001.h"
#include "rkMenu\rkMenu001.h"

//Convenience macros:

#define setLDrive(val) (motor[flWheel] = motor[blWheel] = val) //Set the left wheels
#define setRDrive(val) (motor[frWheel] = motor[brWheel] = val) //Set the right wheels
#define setDrive(val) setLDrive(setRDrive(val)) //Set all four wheels
#define setPointTurn(val) setRDrive(-setLDrive(val)) //Do a point turn

#define setArms(val) (motor[blArm] = motor[tlArm] = motor[brArm] = motor[trArm] = val) //Set the arm motors
#define setIntake(val) (motor[lIntake] = motor[rIntake] = val) //Set the intake motors

//Comment this line out to disable the autonomous timer

Pid *driveLPid, *driveRPid, *armPid, *intakePid; //PID controllers for the wheels, arm, and intake

bool bLCDTimerUsable = false;

byte autonFlags;

task delayedLCDBacklightOff() {
  bLCDBacklight = false;

task manageLCD() {
  bLCDBacklight = true;

  displayLCDCenteredString(0, "Welcome to");
  displayLCDCenteredString(1, "Schillingbot");



    MenuItem autonSelectItems[4];
    autonSelectItems[0].text = "Red Pole";
    autonSelectItems[1].text = "Red Skyrise";
    autonSelectItems[2].text = "Blue Pole";
    autonSelectItems[3].text = "Blue Skyrise";

    autonFlags = showMenu(autonSelectItems, 4);

    displayLCDString(0, 0, autonSelectItems[autonFlags].text);

  bLCDTimerUsable = true;

  string battText;
  bool bklight, lastBklight;
  while(true) {
    bklight = !bIfiRobotDisabled || nLCDButtons;

    if(bklight ^ lastBklight) {
      if(bklight) { bLCDBacklight = true; stopTask(delayedLCDBacklightOff); }
      else startTask(delayedLCDBacklightOff);


    sprintf(battText, "%1.2f V / %1.2f V", nImmediateBatteryLevel / 1000., BackupBatteryLevel / 1000.);

    displayLCDString(1, 0, battText);


    lastBklight = bklight;

//Runs at program start
void init() {
  clearAll(actOnVariables | actOnTimers | actOnMotors | actOnSensors); //Clear ALL the things

  //Set the autonomous flags to something clearly invalid, since they were cleared with clearAll
  //This is used to indicate that autonomous should NOT be started
  autonFlags = 0xFF;

  //Start displaying stuff

  //Initialize the PID controller for the left wheels
  //Master: back left wheel
  //Slaves: front left wheel
  addPid(&driveLPid, blWheel, 30, .75, .1, .05);
  addPidSlave(driveLPid, flWheel);

  //Initialize the PID controller for the right wheels
  //Master: back right wheel
  //Slaves: front right wheel
  addPid(&driveRPid, brWheel, 30, .75, .1, .05);
  addPidSlave(driveRPid, frWheel);

  //Initialize the PID controller for the arms
  //Master: bottom left arm motor
  //Slaves: bottom right, top left, and top right arm motors
  addPid(&armPid, blArm, 30, .8, .165, .05);
  addPidSlave(armPid, brArm);
  addPidSlave(armPid, tlArm);
  addPidSlave(armPid, trArm);

  //Initialize the PID controller for the intake
  //Master: right intake motor
  //Slaves: left intake motor
  addPid(&intakePid, rIntake, 30, -.8, -.05, -.01);
  addPidSlave(intakePid, lIntake);

  //Lower the arms, in case they're raised

  //Wait up to ten seconds for the arms to hit the limit switch
  blockLim(10000, !SensorValue[armLim]);

  //Stop the arms

  //Give the arms time to settle to their natural stop

  //Set the arm encoder to 0

//Cheater-cube autonomous routine
void autonChCube() {
  //Raise arms to drop cheater-cube
  movePid(armPid, 300);

  blockLim(1000, !armPid->isOnTarget);

  movePid(armPid, 0);

  blockLim(1000, !armPid->isOnTarget);

//Stopwatch for debugging autonomous
task debugTime() {
  string timeText;
  while(true) {
    if(bLCDTimerUsable) {

      sprintf(timeText, "%02i:%02d.%03d S", time1[T2] / 60000, (time1[T2] / 1000) % 60, time1[T2] % 1000);

      displayLCDString(0, 0, timeText);


//Three-cube autonomous routine when starting next to a post
//doFlip:  Set to true to flip left and right drive
void autonCubesPost(bool doFlip) {
  Pid *driveAPid = doFlip ? driveRPid : driveLPid,
    *driveBPid = doFlip ? driveLPid : driveRPid;

  //Drive forward to first cube
  movePid(driveAPid, 230, 80);
  movePid(driveBPid, doFlip ? 235 : 230, 80);

  blockLim(2000, !driveAPid->isOnTarget || !driveBPid->isOnTarget);

  //Pick up first cube
  movePid(intakePid, -450);

  blockLim(2000, !intakePid->isOnTarget);

  //Swing turn 45 degrees
  movePidBy(driveAPid, 0, 95);
  movePidBy(driveBPid, doFlip ? 300 : 280, 95);

  blockLim(2000, !driveBPid->isOnTarget);

  //Move forward at an angle
  movePidBy(driveAPid, 350);
  movePidBy(driveBPid, 350);

  blockLim(2000, !driveAPid->isOnTarget || !driveBPid->isOnTarget);

  //Swing turn 45 degrees again
  movePidBy(driveBPid, doFlip ? 250 : 230);

  blockLim(2000, !driveBPid->isOnTarget);

  //Approach second cube
  movePidBy(driveAPid, doFlip ? 110 : 80);
  movePidBy(driveBPid, doFlip ? 110 : 80);

  blockLim(2000, !driveAPid->isOnTarget || !driveBPid->isOnTarget);

  //Pick up second cube
  movePidBy(intakePid, -460);


  //Raise arms to third cube
  movePid(armPid, 360);

  blockLim(2000, !armPid->isOnTarget);

  //Approach third cube
  movePidBy(driveAPid, 160);
  movePidBy(driveBPid, 160);

  blockLim(2000, !driveAPid->isOnTarget || !driveBPid->isOnTarget);

  //Pick up third cube
  movePidBy(intakePid, -220);

  blockLim(500, !intakePid->isOnTarget);

  //Back up
  movePidBy(driveAPid, -200);
  movePidBy(driveBPid, -200);

  blockLim(2000, !driveAPid->isOnTarget || !driveBPid->isOnTarget);

  //Point turn 90 degrees
  movePidBy(driveAPid, -300);
  movePidBy(driveBPid, 300);

  blockLim(2000, !driveAPid->isOnTarget || !driveBPid->isOnTarget);

  //Move left of the pole
  movePidBy(driveAPid, 75);
  movePidBy(driveBPid, 75);

  blockLim(2000, !driveAPid->isOnTarget || !driveBPid->isOnTarget);

  //Swing turn forward 90 degrees
  movePidBy(driveBPid, -300);
  movePidBy(driveBPid, 300);

  blockLim(2000, !driveBPid->isOnTarget);

  //Raise the arm
  movePid(armPid, 1030);

  blockLim(2000, !armPid->isOnTarget);

  //Move toward the pole
  movePidBy(driveAPid, 250, 55);
  movePidBy(driveBPid, 250, 55);

  blockLim(2000, !driveAPid->isOnTarget || !driveBPid->isOnTarget);


  //Dump 'em
  movePidBy(intakePid, 1000);

  blockLim(2000, !intakePid->isOnTarget);

  //Back up and lower arm
  movePidBy(driveAPid, -100, 80);
  movePidBy(driveBPid, -100, 80);
  movePid(armPid, 0, 63);
  movePid(intakePid, 0, 63);

  blockLim(2000, !driveAPid->isOnTarget || !driveBPid->isOnTarget || !armPid->isOnTarget || !intakePid->isOnTarget);

//Autonomous routine
task auton() {
  //Enable the PID controllers.
  //Passing true as the second argument resets the encoder position.
  startPid(driveLPid, true);
  startPid(driveRPid, true);
  startPid(armPid); //Calibrated during the init() function.  Does not need resetting.
  startPid(intakePid, true);

  block(autonFlags == 0xFF);

  //Begin the stopwatch

  //Run the cheater-cube routine
  if(!(autonFlags & 0b01))autonChCube();

  //Run the three-cube routine
  autonCubesPost(autonFlags ^ (autonFlags >> 1));

  //Clean up when finished

//Clean up when autonomous ends
void endAuton() {
  //Stop the timer
  //Disable the PID controllers and stop the task

task userOp() {
  //Velocity for the arms and intake
  short digiVelArm, digiVelIntake;

  while (true) {
    //Tank drive
  setRDrive(abs(vexRT[Ch2]) > 3 ? vexRT[Ch2] : 0); //Tank right
  setLDrive(abs(vexRT[Ch3]) > 3 ? vexRT[Ch3] : 0); //Tank left

    //Set the arm and intake velocities, controlled by channels 7D and 8D
  digiVelArm = vexRT[Btn7D] ? 63 : 127; //Arm velocity
  digiVelIntake = vexRT[Btn8D] ? 63 : 127; //Intake velocity

    //Digital control of the arms, using constant power, the limit switch, and controlled velocity (with 7D)
  setArms((vexRT[Btn5U] ^ vexRT[Btn5D]) ? (vexRT[Btn5U] ? digiVelArm : SensorValue[armLim] ? 0 : -digiVelArm) : SensorValue[armLim] ? -10 : 20);

    //Digital control of the intake, using constant power and controlled velocity (with 8D)
  setIntake((vexRT[Btn6U] ^ vexRT[Btn6D]) ? (vexRT[Btn6U] ? digiVelIntake : -digiVelIntake) : 10);

//(unused) Clean up when user-op ends.
void endUserOp() { }

And here are the header files.
#include "rkUtil001.h"

//Parameters and variables used by a single PID controller
typedef struct {
  //master: The master motor with the encoder
  //slaves: The slaves to follow it
  tMotor master, slaves[10];

  //slaveCount: The number of slaves attached to the master
  //index: The index of this Pid in the list;
  int slaveCount, index;

  //kP: The proportional coefficient
  //kI: The integral coefficient
  //kD: The derivative coefficient
  //lastInteg: The last sum of error (for integral)
  float kP, kI, kD, lastInteg;

  //setPoint: The target rotation
  //lastError: The last error value (for derivative)
  //thresh: Threshold within which the motor does not move
  //lim:  Upper/lower power limit
  short setPoint, lastError, thresh, lim;

  //last time (for calculating delta)
  long lastTime;

  //doRun: True if PID should be evaluated and updated
  //isOnTarget: True if PID is within threshold
  bool doRun, isOnTarget;
} Pid;

//Represents the values generated by calcPid(...);
typedef struct {
  //val: The value returned from the calculation
  //error: The error calculated by the function
  short val, error;

  //integ: The integral calculated by the function
  float integ;

  //time: The time recorded by the function
  long time;

  //isOnTarget: was the error within the threshold?
  bool isOnTarget;
} PidCalc;

Pid pids[10]; //This is where the Pids are stored
int pidCount = 0; //How many Pids we actually have (10 is a max value)

short getPidEncoder(Pid* pid) { return nMotorEncoder[pid->master]; } //Get the encoder value for a Pid

//Given all the variables and constants, calculates the values for a PID controller and returns them in a PidCalc
void calcPid(PidCalc* calc, short thresh, short lim, short value, short setPoint, short lastError, float lastInteg, long time, long lastTime, float kP, float kI, float kD) {
  calc->error = value - setPoint; //Get the error from the setpoint
  calc->time = time; //Return the time, so it can be used again as lastTime
  float deltaTime = (time - lastTime) / 1000.; //Time between lastTime and time, used for I and D

  calc->isOnTarget = abs(calc->error) <= thresh; //Indicates if the error is within the threshold
  //Clear the value and integral (i.e. complete stop) and stop here if on target
  if(calc->isOnTarget) {
    calc->val = 0;
    calc->integ = 0;

  calc->integ = lastInteg + (kI * (float)calc->error * deltaTime); //Calculate the integral value for use again as lastInteg

  calc->val = max(-lim, min(lim, kP * -calc->error)); //Calculate proportional power

  calc->val = max(-lim, min(lim, calc->val - calc->integ)); //Calculate integral (accumulated) power

  if(time != lastTime) calc->val = max(-lim, min(lim, calc->val + kD * ((float)lastError - (float)calc->error) / deltaTime)); //Calculate derivative (frictional) power

//Get a PidCalc from a Pid and process its values
short calcPid(Pid* pid) {
  PidCalc calc;

  calcPid(&calc, pid->thresh, pid->lim, getPidEncoder(pid), pid->setPoint, pid->lastError, pid->lastInteg, time1[T4], pid->lastTime, pid->kP, pid->kI, pid->kD);

  pid->lastError = calc.error;
  pid->lastInteg = calc.integ;
  pid->lastTime = calc.time;
  pid->isOnTarget = calc.isOnTarget;

  return calc.val;

//Set the master and slave motor powers of a Pid to val
void setPidMotor(Pid* pid, short val) {
  motor[pid->master] = val;

  for(int i = 0; i < pid->slaveCount; i++)
    motor[pid->slaves[i]] = val;

//Calculate a Pid and apply the value to its motors
void updatePid(Pid* pid) {
  setPidMotor(pid, calcPid(pid));

bool doUpdatePids = false; //Is the updatePids task running?
//PID processing loop
task updatePids() {
  while(doUpdatePids) {
    for(int i = 0; i < pidCount; i++) {

//Initialize a Pid and add it to the list
void addPid(Pid** pidRef, tMotor master, short thresh, float kP, float kI, float kD) {
  if(pidCount >= 10) return;

  pids[pidCount].index = pidCount;
  pids[pidCount].master = master;
  pids[pidCount].kP = kP;
  pids[pidCount].kI = kI;
  pids[pidCount].kD = kD;
  pids[pidCount].setPoint = 0;
  pids[pidCount].lastError = 0;
  pids[pidCount].thresh = thresh;
  pids[pidCount].lim = 127;

  *pidRef = &pids[pidCount];


//Assign a slave motor to a Pid
void addPidSlave(Pid* pid, tMotor slave) {
  if(pid->slaveCount >= 10) return;

  for(int i = 0; i < pid->slaveCount; i++)
    if(pid->slaves[i] == slave) return;

  pid->slaves[pid->slaveCount++] = slave;

//Remove a Pid from the list
void removePid(Pid* pid) {
  for(int i = pid->index + 1; i < pidCount; i++) {
    pids[i].index = i - 1;
    pids[i - 1] = pids[i];


//Reset a Pid's encoder and setpoint
void resetPid(Pid* pid) {
  nMotorEncoder[pid->master] = 0;
  pid->setPoint = 0;

//Begin running a Pid
void startPid(Pid* pid) {
  if(!doUpdatePids) {
    doUpdatePids = true;
  pid->doRun = true;

//Begin running a Pid, and reset it if doReset is true
void startPid(Pid* pid, bool doReset) {
  if(doReset) resetPid(pid);

//Update a Pid's setpoint
void movePid(Pid* pid, short setPoint) {
  if(!pid->doRun) return;
  pid->doRun = false;
  pid->setPoint = setPoint;
  pid->lastInteg = 0;

  pid->doRun = true;

//Update a Pid's setpoint and power limit
void movePid(Pid* pid, short setPoint, short lim) {
  if(!pid->doRun) return;
  pid->lim = lim;
  movePid(pid, setPoint);

//Shift a Pid's setpoint by a value
void movePidBy(Pid* pid, short by) {
  movePid(pid, pid->setPoint + by);

//Shift a Pid's setpoint by a value and update its power limit
void movePidBy(Pid* pid, short by, short lim) {
  movePid(pid, pid->setPoint + by, lim);

//Stop calculating a Pid
void stopPid(Pid* pid) {
  if(doUpdatePids) {
    bool areAnyPidsRunning = false;
    for(int i = 0; !areAnyPidsRunning && i < pidCount; i++)
      if(pids[i].doRun) areAnyPidsRunning = true;

    if(!areAnyPidsRunning) {
      doUpdatePids = false;

  pid->doRun = false;
  setPidMotor(pid, 0);

//Stop calculating all the Pids
void stopAllPids() {
  doUpdatePids = false;
  for(int i = 0; i < pidCount; i++) {
    pids[i].doRun = false;
    setPidMotor(pids[i], 0);

#define max(a, b) (a > b ? a : b) //Returns the greater of the two values
#define min(a, b) (a > b ? b : a) //Returns the lesser of the two values

#define block(cond) while(cond) EndTimeSlice() //Simple multi-task-friendly blocking loop; blocks infinitely while cond is met

#define whileLim(lim, cond) clearTimer(T1); while((cond) && time1[T1] <= lim) //Loops for up to lim milliseconds while cond is met

#define blockLim(lim, cond) whileLim(lim, cond) EndTimeSlice() //Blocks for up to lim milliseconds while cond is met

#define clearLCD() clearLCDLine(0); clearLCDLine(1) //Clears both LCD lines

typedef char byte;

Mon Apr 13, 2015 10:13 pm

Joined: Mon Apr 13, 2015 9:58 pm
Posts: 3
Post Re: Autonomous Does not Function Properly
#pragma platform(VEX)
#pragma competitionControl(Competition);

//Forward definitions of the necessary functions
void init(); //Runs at the program's start
void endAuton(); //Runs when the autonomous period ends
void endUserOp(); //Runs when the user-op period ends
task auton(); //Runs during the autonomous period
task userOp(); //Runs during the user-op period

task main() {

   while(true) {
      while(bIfiRobotDisabled) { wait1Msec(25); EndTimeSlice(); } //Wait until the robot isn't disabled

      //Are we in autonomous now?
      if(bIfiAutonomousMode) {
         startTask(auton); //Begin the autonomous routine

         while(bIfiAutonomousMode && !bIfiRobotDisabled) { wait1Msec(25); EndTimeSlice(); } //Wait until autonomous ends or the robot's disabled

         stopTask(auton); //End the autonomous routine
         endAuton(); //Clean up afterwards
      else { //We must be in user-op
        startTask(userOp); //Begin the user-op routine

        while(!bIfiAutonomousMode && !bIfiRobotDisabled) { wait1Msec(25); EndTimeSlice(); } //Wait until user-op ends or the robot's disabled

        stopTask(userOp); //End the user-op routine
        endUserOp(); //Clean up afterwards

#include "rkUtil001.h"

#define lbtn(flags) (flags & kButtonLeft)
#define cbtn(flags) (flags & kButtonCenter)
#define rbtn(flags) (flags & kButtonRight)

typedef struct {
  string text;
} MenuItem;

const int carets[2] = { ' ', '>' };

void dispMenuCaret(bool caretPos) {
  displayLCDChar(!caretPos, 0, ' ');
  displayLCDChar(caretPos, 0, '>');

void dispMenu(MenuItem *items, int scroll, bool caretPos) {

  displayLCDString(0, 1, items[scroll].text);
  displayLCDString(1, 1, items[scroll + 1].text);


int showScrollingMenu(MenuItem *items, int itemCount) {
  int scroll = 0, maxScroll = itemCount - 2;
  bool caretPos = false;

  TControllerButtons lastBtns = 0x0;

  dispMenu(items, scroll, caretPos);

  while(true) {
    if(!cbtn(nLCDButtons) && cbtn(lastBtns)) {
      return scroll + caretPos;
    if(lbtn(nLCDButtons) && !lbtn(lastBtns)) {
      if(caretPos) {
        caretPos = false;
      else {
        if(!scroll) { scroll = maxScroll; caretPos = true; }
        else scroll--;
        dispMenu(items, scroll, caretPos);
    if(rbtn(nLCDButtons) && !rbtn(lastBtns)) {
      if(caretPos) {
        if(scroll == maxScroll) { scroll = 0; caretPos = false; }
        else scroll++;
        dispMenu(items, scroll, caretPos);
      else {
        caretPos = true;

    lastBtns = nLCDButtons;


  return 0;

int showBinaryMenu(MenuItem *items) {
  TControllerButtons lastBtns = 0x0;
  bool caretPos = false;

  dispMenu(items, 0, caretPos);

  while(true) {
    if(!cbtn(nLCDButtons) && cbtn(lastBtns)) break;
    if((nLCDButtons & !lastBtns) & (kButtonLeft | kButtonRight)) {
      caretPos = !caretPos;

    lastBtns = nLCDButtons;


  return caretPos;

int showMenu(MenuItem *items, int itemCount) {
  switch(itemCount) {
    case 0:
    case 1:
      return 0;
    case 2:
      return showBinaryMenu(items);
      return showScrollingMenu(items, itemCount);

Mon Apr 13, 2015 10:16 pm
Display posts from previous:  Sort by  
Reply to topic   [ 3 posts ] 

Who is online

Users browsing this forum: No registered users and 2 guests

You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot post attachments in this forum

Search for:
Jump to:  

Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group.
Designed by ST Software for PTF.