RobotC code for potentiometer & Motor Controller issue

TeamTX · January 24, 2018, 6:37pm

@jpearman
We added RobotC code for a potentiometer on our chainbar with preset buttons based off code that jpearman had posted online with modifications to work with our robot (added another preset, set it to hold going both forward and backward by checking where the pot was set, adjusting motors).

On Sunday after editing the code, everything worked great for a short period and then we started having multiple issues. We aren’t sure if this is code related or motors / wiring related, so while we work on other fixes we’re hoping someone can take a look at the code and make sure we integrated it with our current drive program correctly. We haven’t worked with multiple tasks before, and this is more complicated code than we have been using up to now (although we walked through it all very carefully and are pretty sure we followed along).

Original main issue: when using preset buttons, the mobile goal carrier seems to start moving in and out a little bit and then stall (all on its own with no buttons pressed).
– Does this have something to do with the presets being blocking code? Isn’t having it in a separate task supposed to let other things operate fairly normally?
New main issue: many of the motor controllers are suddenly shorting out between Sunday and Wednesday. Since the original problem Sunday, we had 3 different motors stop working correctly and we determined it was likely motor controller issues; the 2 we had available to replace them with also didn’t work.
– Motors will start only going one direction
– Motors will start being very slow / weak
– Motors will stop working altogether
– Some Motor Controllers will start getting very hot to the touch (much hotter than other MC running)

We’re running RobotC 4.56. We have competed since October with this robot (although we started with the base and then added the lift portion in December) and have made changes but nothing that we think would cause these sudden issues. We have 12 motors and a power expander.

While talking with tech support about motor controllers we were told that they don’t usually go bad or get very hot, and that if they do it is likely a faulty wiring issue causing a short. They also seemed to be saying bad motor controllers usually melt the plastic and can be seen from the outside, though, and we tested 17 motor controllers today that all looked fine (although 2 were tested after repairs) and only 8 worked correctly. Anyhow, we:

Removed all wires and motor controllers from the robot
Tested and checked all wires (seemed fine), tested all motor controllers (only 8 worked), tested all motors on the robot (all but 2 worked as expected; 1 of those was running slow and was a drive motor so we replaced it - the other 1 worked well only when the wire was pointing up so we zip tied it into place)
Added back all base motor wiring (drivetrain, mobile goal lift/grab); the base seems to be working now (limited tests due to time that it took to do all the work!), but that is without any of the top elements attached.

Tomorrow we’ll begin re-wiring the top section, but we’re concerned that if faulty code could have caused some problems if we don’t get that fixed we may just end up having issues again. The timing was suspicious, as code was pretty much the only thing we were really working on recently. Code posted next.

tabor473 · January 24, 2018, 6:44pm

Post the code.

Make sure the motors don’t have broken or frayed wires. Wires shorting is main cause of broken MC.

TeamTX · January 24, 2018, 6:45pm


#pragma config(Sensor, in1,    POT,            sensorPotentiometer)
#pragma config(Motor,  port1,           rightMotorB,   tmotorVex393_HBridge, openLoop, reversed, driveRight)
#pragma config(Motor,  port2,           rightMotorF,   tmotorVex393_MC29, openLoop, reversed, driveRight)
#pragma config(Motor,  port3,           ConeLift,      tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port4,           SissorRight,   tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port5,           SissorLeft,    tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port6,           ClawMotor,     tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port7,           ArmMotors,     tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port8,           ConeGrabber,   tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port9,           leftMotorF,    tmotorVex393_MC29, openLoop, driveLeft)
#pragma config(Motor,  port10,          leftMotorB,    tmotorVex393_HBridge, openLoop, driveLeft)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*        Description: Competition template for VEX EDR                      */
/*                                                                           */
/*---------------------------------------------------------------------------*/

// This code is for the VEX cortex platform
#pragma platform(VEX2)

// Select Download method as "competition"
#pragma competitionControl(Competition)

//Main competition background code...do not modify!
#include "Vex_Competition_Includes.c"

/*---------------------------------------------------------------------------*/
/*                          Pre-Autonomous Functions                         */
/*                                                                           */
/*  You may want to perform some actions before the competition starts.      */
/*  Do them in the following function.  You must return from this function   */
/*  or the autonomous and usercontrol tasks will not be started.  This       */
/*  function is only called once after the cortex has been powered on and    */
/*  not every time that the robot is disabled.                               */
/*---------------------------------------------------------------------------*/

void pre_auton()
{
  // Set bStopTasksBetweenModes to false if you want to keep user created tasks
  // running between Autonomous and Driver controlled modes. You will need to
  // manage all user created tasks if set to false.
  bStopTasksBetweenModes = true;

	// Set bDisplayCompetitionStatusOnLcd to false if you don't want the LCD
	// used by the competition include file, for example, you might want
	// to display your team name on the LCD in this function.
	// bDisplayCompetitionStatusOnLcd = false;

  // All activities that occur before the competition starts
  // Example: clearing encoders, setting servo positions, ...
}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              Autonomous Task                              */
/*                                                                           */
/*  This task is used to control your robot during the autonomous phase of   */
/*  a VEX Competition.                                                       */
/*                                                                           */
/*  You must modify the code to add your own robot specific commands here.   */
/*---------------------------------------------------------------------------*/

int RotationTime;

void MoveForward (int Speed, int DIS)
{


	RotationTime = DIS * 24.78;
	setMultipleMotors(Speed, rightMotorB, rightMotorF, leftMotorB, leftMotorF);
	wait1Msec(RotationTime);
	stopMultipleMotors(rightMotorB, rightMotorF, leftMotorB, leftMotorF);
}

void MoveBackward (int Speed, int DIS)
{


	RotationTime = DIS * 24.78;
	setMultipleMotors(-Speed, rightMotorB, rightMotorF, leftMotorB, leftMotorF);
	wait1Msec(RotationTime);
	stopMultipleMotors(rightMotorB, rightMotorF, leftMotorB, leftMotorF);
}
task autonomous()
{
  // ..........................................................................
  // Insert user code here.
  // ..........................................................................

// auto code removed
}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              User Control Task                            */
/*                                                                           */
/*  This task is used to control your robot during the user control phase of */
/*  a VEX Competition.                                                       */
/*                                                                           */
/*  You must modify the code to add your own robot specific commands here.   */
/*---------------------------------------------------------------------------*/

// Joystick for manual control
#define     JOY_ARM             Ch2Xmtr2
#define     JOY_THRESHOLD       10

// buttons for preset arm control
#define     ARM_PST_BUTTON_1    Btn5UXmtr2 //Cone Grabbing
#define     ARM_PST_BUTTON_2    Btn7LXmtr2 //Driving Around
#define     ARM_PST_BUTTON_3    Btn5DXmtr2 //Cone Dropping On Goal
#define     ARM_PST_BUTTON_4    Btn7UXmtr2 //Getting Cone From Loader

// Preset positions and limits
#define     ARM_LOWER_LIMIT     325
#define     ARM_GOAL_POS        425
#define     ARM_DRIVE_POS       1500
#define     ARM_CONE_GRABBING   3885
#define     ARM_LOADING_POS     3300
#define     ARM_UPPER_LIMIT     4200

#define     ARM_HOLD_POWER      20
#define     ARM_HOLD_POWER_NEG -20

#define     straight_up 				1850

/*-----------------------------------------------------------------------------*/
/*    Drive arm motors                                                         */
/*-----------------------------------------------------------------------------*/

void DriveArmMotor( int value )
{
    // drive all the arm motors here
    motor port3 ] = value;
}

/*-----------------------------------------------------------------------------*/
/*    Create arm drive based on a requested position                           */
/*-----------------------------------------------------------------------------*/

int ArmAutoDrive( int target_position, int arm_position)
{
    int arm_drive;

    // 100 is the tolerance of the acceptable error
    // too small and the arm will oscillate
    if( abs( target_position - arm_position ) < 100 && arm_position < straight_up)
        {
        // here is where you set a non zero value to hole the arm if necessary
        // any larger than 20 and you are in danger of tripping PTCs
        arm_drive = ARM_HOLD_POWER;
        }
    else
    	if( abs( target_position - arm_position ) < 100 && arm_position > straight_up)
        {
        // here is where you set a non zero value to hole the arm if necessary
        // any larger than 20 and you are in danger of tripping PTCs
        arm_drive = ARM_HOLD_POWER_NEG;
        }
    else
    // Do we need to go up
    if( target_position > arm_position )
        {
        // this is proportional control, Kp is 0.1 here
        arm_drive = 0.1 * (float)(target_position - arm_position);

        // limit to thr maximum speed you want
        if(arm_drive > 127)
            arm_drive = 127;
        // Limit to the minimum speed that will move your arm
        if(arm_drive < 32)
            arm_drive = 32;
        }
    else
    // Do we need to go down
    if( target_position < arm_position )
        {
        // move downwards, use different Kp if necessary
        arm_drive = -0.1 * (float)(arm_position - target_position);

        // limit to maximum downward speed here
        if(arm_drive < (-127))
            arm_drive = (-127);
        // Limit to the minimum speed that will move your arm
        if(arm_drive > (-32))
            arm_drive = (-32);
        }
    else
        arm_drive = 0; // error

    // return the calculated value for the arm drive
    return arm_drive;
}

TeamTX · January 24, 2018, 6:48pm



/*-----------------------------------------------------------------------------*/
/*  Driver control for arm                                                     */
/*-----------------------------------------------------------------------------*/

task DriverArmTask()
{
    int   arm_drive;
    int   arm_position;
    static  int   arm_target = 0;


    // Basic Lift & intake control
    while( true )
        {
        // Get arm joystick value
        arm_drive = vexRT JOY_ARM ];

        // Get arm potentiometer value
        // down should be low values, up should be high values
        // if it is inverted then use arm_position = 4095 - SensorValue( armPot );
        arm_position = SensorValue( POT );

        // Ignore joystick near center
        if( (arm_drive <= JOY_THRESHOLD) && (arm_drive >= -JOY_THRESHOLD))
            {
            if( vexRT ARM_PST_BUTTON_3 ] == 1 )
                arm_target = ARM_GOAL_POS;
            if( vexRT ARM_PST_BUTTON_2 ] == 1 )
                arm_target = ARM_DRIVE_POS;
            if( vexRT ARM_PST_BUTTON_1 ] == 1 )
                arm_target = ARM_CONE_GRABBING;
            if( vexRT ARM_PST_BUTTON_4 ] == 1 )
                arm_target = ARM_LOADING_POS;


            // else no drive - send holding power for manual
            if (arm_position < straight_up)
            arm_drive = ARM_HOLD_POWER;
            if (arm_position > straight_up)
            arm_drive = ARM_HOLD_POWER_NEG;
            }
        else
            {
            // clear target position
            arm_target        = 0;
            }

        // if using a preset get arm_drive
        if( arm_target != 0 )
            arm_drive = ArmAutoDrive( arm_target, arm_position );

        // send at least the holding power to the motor
        if( (sgn(arm_drive) == 1) && (arm_drive < ARM_HOLD_POWER) && ( arm_position < straight_up))
            arm_drive = ARM_HOLD_POWER;

           if( (sgn(arm_drive) == 1) && (arm_drive < ARM_HOLD_POWER) && ( arm_position > straight_up))
            arm_drive = ARM_HOLD_POWER_NEG;

        // limit drive so we stop at absolute top and bottom
        if( ( arm_position > ARM_UPPER_LIMIT ) && (arm_drive > 0) )
            arm_drive = 0;
        if( ( arm_position < ARM_LOWER_LIMIT ) && (arm_drive < 0) )
            arm_drive = 0;

        // Drive motor
        DriveArmMotor( arm_drive );

        // don't hog CPU
        wait1Msec( 25 );
    }
}

task usercontrol()
{

  // User control code here, inside the loop
  int threshold = 10;

  while (true)
  {
  	    startTask( DriverArmTask );
    // This is the main execution loop for the user control program.
    // Each time through the loop your program should update motor + servo
    // values based on feedback from the joysticks.

    // ........................................................................
    // Insert user code here. This is where you use the joystick values to
    // update your motors, etc.
    // ........................................................................

    if(abs(vexRT[Ch3]) > threshold)
    {
      motor[leftMotorF]  = (vexRT[Ch3]);
    }
    else
    {
      motor[leftMotorF]  = 0;
    }
    if(abs(vexRT[Ch3]) > threshold)
    {
      motor[leftMotorB]  = (vexRT[Ch3]);
    }
    else
    {
      motor[leftMotorB]  = 0;
    }

    if(abs(vexRT[Ch2]) > threshold)
    {
      motor[rightMotorF] = (vexRT[Ch2]);
    }
    else
    {
      motor[rightMotorF] = 0;
    }
        if(abs(vexRT[Ch2]) > threshold)
    {
      motor[rightMotorB] = (vexRT[Ch2]);
    }
    else
    {
      motor[rightMotorB] = 0;
    }
    	if(vexRT[Btn6U] == 1)
		{
		motor[ArmMotors] = 100;

		}
		else if(vexRT[Btn6D] == 1)
		{
			motor[ArmMotors] = -100;

		}
		else
		{
    motor[ArmMotors] = 0;
		}
		    	if(vexRT[Btn5U] == 1)
		{
		motor[ClawMotor] = 127;

		}
		else if(vexRT[Btn5D] == 1)
		{
			motor[ClawMotor] = -127;
		}
		else
		{
		motor[ClawMotor] = -10;
		{
		//if(abs(vexRT[Ch2Xmtr2]) > threshold)
		//	{
		//motor[ConeLift] = (vexRT[Ch2Xmtr2]);
		//}
		//else if(vexRT[Btn5UXmtr2] == 1)
		//{
		//motor[ConeLift] = 100;

		//}
		//else if(vexRT[Btn5DXmtr2] == 1)
		//{
		//	motor[ConeLift] = -100;
		//}
		//else
		//{
  //    motor[ConeLift] = 10;
		//}
//scissor lift - both remotes
		if((vexRT[Btn8D] == 1) || (vexRT[Btn8DXmtr2] == 1))
		{
		  motor[SissorRight] = 127;
		  motor[SissorLeft] = 127;
		}
		else if((vexRT[Btn7D] == 1) || (vexRT[Btn7DXmtr2] == 1))
		{
			motor[SissorRight] = -127;
			motor[SissorLeft] = -127;

		}
		else
		{
      motor[SissorRight] = 0;
      motor[SissorLeft] = 0;
		}
//cone grabber/intake
		if(vexRT[Btn6UXmtr2] == 1)
		{
		  motor[ConeGrabber] = 100;
		}
		else if(vexRT[Btn6DXmtr2] == 1)
		{
			motor[ConeGrabber] = -100;

		}
		else
		{
      motor[ConeGrabber] = 20;
		}
  	}
	}

}

}

TeamTX · January 24, 2018, 6:54pm

The motor wires seem fine on the outside - do we need to remove them and open them all up to check inside? Eek!

TeamTX · January 25, 2018, 3:38pm

Any additional thoughts, by any chance?

We’ll be finishing up re-wiring tonight and put the code back on - is there any reason to assume the motor controller issue would be with coding vs with wiring/other? We’re not sure if it is even possible to mess up motor controllers with code. We have a competition coming up Sat and we don’t want to accidentally mess up the new MC! We’ll double check wiring on motors and between everything again as we go, and we’ll be using cable management this time, so if it is bad wires hopefully it will be fixed.

For the other problem, with random movement of a motor in a different task, we haven’t had the opportunity to do any more troubleshooting due to all the other issues.

biglesliep · January 27, 2018, 10:48am

You seem to have some syntax problems in your code, but I’m not sure that it’s the source of any problem, or of these items are an artifact of copying & pasting the code here.

The following line is missing some partnetheses

else if( abs( target_position - arm_position ) < 100 && arm_position > straight_up)

Should really be like so, with each comparison statement in its own parentheses

else if( (abs( target_position - arm_position ) < 100) && (arm_position > straight_up) )

The closing bracket here is backward

else
		{
		motor[ClawMotor] = -10;
		{

For the “Original issue”, you say that the MG carrier moves by itself, but from your code it’s not clear which item affects mobile goal: you’ve got ConeLift & ConGrabber – obvious what these are; then you’ve got Claw, Arm, and Scissor – which of these is used to move the mobile goal? and which of these motors is the one that operates by itself?

For the “new issue”, it sounds like motor stall. Are you sure that the motors dedicated to the task are powerful enough to do what you’re asking? It’s kind of classic underpowered-mechanism behavior.

Hope this helps.

jpearman · January 27, 2018, 1:58pm

Although I usually add parenthesis for completeness it actually doesn’t matter in this case. The > and < operators have higher precedense than the &&.

TeamTX · January 27, 2018, 6:18pm

@biglesliep - We’ll fix those, thanks!

It largely seems to be with the presets on that program, as at the competition and in practice we had few problems and we just avoided using the presets much. We’d like to get those working, now that we’ve got a bit more time for specific program testing! Is there anything in particular we should try?

Scissor = scissor lift; Arm = mobile goal lifting arm; Claw = mobile goal grabber at end of arm
The Arm is what seems to move strangely when presets are used; it is supposed to reach forward, then the claw grabs the goal and holds, then the arm pulls it back to rest inside the robot. Under normal driving, this seems fine.
When using presets, after it reaches forward and grabs then pulls back inside the robot (standard driving move) and you push the preset (to get match loads from platform with 1 button then forward to drop onto goal with 2nd button) it shifts the Arm forward a tiny bit then back again. The claw then tips forward under the weight of the goal (not sure if it is releasing the claw hold or just due to heavy weight); this causes the alignment to change and the cone to not drop directly onto it.
The problem is more noticeable once the cone stack is bigger and we also have to raise the scissor lift up some for height. It might even be more of an argument between the scissor lift and the arm… hmmm…

We’re pretty sure that the “new issue” was faulty wiring and motor controller shorts rather than stalling due to under-powered. After replacing all the motor controllers and wiring this week with confirmed-working ones everything has been working well. We ended up doing quite well at the competition today!

We replaced the 1 motor that had to have the wires pointed up; when we re-wired and tested this one, we actually saw the motor controller on it start smoking! Must have been bad wires inside the motor, as outside of it they looked fine.

system · January 3, 2020, 10:05pm