[FTC4211]

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