[FTC]: Problem with RobotC servo code

Other FIRST Tech Challenge
#1

I’m currently trying to program my team’s robot’s arm. The arm has two servos. I want each servo to move up or down and then stay in the position it moves to using the d-pad or using the A and B buttons. This is the code I’m using - the part of the code that controls servoWrist works, but the part that controls servoElbow does not. I’m fairly certain it’s not a problem with hardware - I’ve tested three different servos using each section of code and all three work with the servoWrist code, but not with the servoElbow code. I also tried using port three instead of port two for the servoElbow code, but that did not help either, so I believe it’s not an issue with the servo controller. I’m currently really confused by this because as far as I can tell, the two sections of code are identical.

#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C2_1,    servoWrist,           tServoStandard)
#pragma config(Servo,  srvo_S1_C2_2,    servoElbow,           tServoStandard)
#pragma config(Servo,  srvo_S1_C2_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//


#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.
void initializeRobot()
{
  return;
}

task main()
{
  initializeRobot();

  waitForStart();   // wait for start of tele-op phase

  while (true)
  {
   getJoystickSettings(joystick);
		int wristVariable;
 	        int elbowVariable;
		int wristHelp;
		int elbowHelp;

		if(joystick.joy1_TopHat == 4)
		{
			wristVariable = wristHelp;
			wristHelp = wristHelp + 1;
			wait1Msec(10);
		}
		
                if(joystick.joy1_TopHat == 0)
		{
			wristVariable = wristHelp;
			wristHelp = wristHelp - 1;
			wait1Msec(10);
		}

			servo[servoWrist]= wristHelp;

		if(joy1Btn(2))
		{
			elbowVariable = elbowHelp;
			elbowHelp = elbowHelp + 1;
			wait1Msec(10);
		}
		if(joy1Btn(4))
		{
			elbowVariable = elbowHelp;
			elbowHelp = elbowHelp - 1;
			wait1Msec(10);
		}

			servo[servoElbow]= elbowHelp;
  }
}
#2

Hello,

One of the problems with the code you have listed is that wristHelp and elbowHelp never gets initialized with a value after it is created at the beginning of the loop and as such their values are undefined, yet you use them to assign a value to the servos.

This code does the same thing functionally as what you wanted yours to do.

#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C2_1,    servoWrist,           tServoStandard)
#pragma config(Servo,  srvo_S1_C2_2,    servoElbow,           tServoStandard)
#pragma config(Servo,  srvo_S1_C2_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//


#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.
void initializeRobot()
{
  return;
}

task main()
{
  initializeRobot();

  waitForStart();   // wait for start of tele-op phase

  while (true)
  {
        getJoystickSettings(joystick);

	if(joystick.joy1_TopHat == 4)
	{
		servo[servoWrist] = ServoValue[servoWrist]+1;
	}
	if(joystick.joy1_TopHat == 0)
	{
		servo[servoWrist] = ServoValue[servoWrist]-1;
	}

	if(joy1Btn(2))
	{
		servo[servoElbow] = ServoValue[servoElbow]+1;
	}
	if(joy1Btn(4))
	{
		servo[servoElbow] = ServoValue[servoElbow]-1;
	}
	wait1Msec(20);
		
  }
}

This code utilizes the built in servo position to increment the servo position. By using the built in variable it skips using all the extra variables minimizing the risk of errors. The delay for your servo movement is now takes care of the movement for both servos and provides a convenient spot to change the servo speeds at one single location rather than having to change it multiple locations.

Hope this works for you,
Team 4211

#3

The code started working after restarting our computer. We’d already tried restarting, but apparently we needed to restart one more time.:stuck_out_tongue: Your code is a much more elegant way of programming the servos though, thanks!

#4