View unanswered posts | View active topics It is currently Thu Aug 21, 2014 4:01 pm






Reply to topic  [ 15 posts ] 
Servos not responding 
Author Message
Rookie

Joined: Sat Oct 26, 2013 11:02 pm
Posts: 8
Post Servos not responding
Okay, well we are pretty much a rookie team/ teams called Technomancers team number 6958 and 7011. team 6958 is Technomancers team alpha, while the other team is beta. Pleased to meet everyone!

Now on to the topic, us two teams are having a problem with our servo programs on our controllers. I am pretty sure they were working while we were practicing with other teams with our half finished robot, but they aren't now. I don't know whether it has to do with me switching a servo with another servo but apparently team 7011 is having a similar problem with far less servos.

Code:
#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     touchsensor,    sensorNone)
#pragma config(Motor,  mtr_S1_C1_1,     leftMotor,     tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     rightMotor,    tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     elbow,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     slider,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C3_1,    shoulder,             tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    wrist,                tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                           Tele-Operation Mode Code Template
//
// This file contains a template for simplified creation of an tele-op program for an FTC
// competition.
//
// You need to customize two functions with code unique to your specific robot.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                    initializeRobot
//
// Prior to the start of tele-op mode, you may want to perform some initialization on your robot
// and the variables within your program.
//
// In most cases, you may not have to add any code to this function and it will remain "empty".
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

void initializeRobot()
{
   
  // Place code here to sinitialize servos to starting positions.
  // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.

  return;
}


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                         Main Task
//
// The following is the main code for the tele-op robot operation. Customize as appropriate for
// your specific robot.
//
// Game controller / joystick information is sent periodically (about every 50 milliseconds) from
// the FMS (Field Management System) to the robot. Most tele-op programs will follow the following
// logic:
//   1. Loop forever repeating the following actions:
//   2. Get the latest game controller / joystick settings that have been received from the PC.
//   3. Perform appropriate actions based on the joystick + buttons settings. This is usually a
//      simple action:
//      *  Joystick values are usually directly translated into power levels for a motor or
//         position of a servo.
//      *  Buttons are usually used to start/stop a motor or cause a servo to move to a specific
//         position.
//   4. Repeat the loop.
//
// Your program needs to continuously loop because you need to continuously respond to changes in
// the game controller settings.
//
// At the end of the tele-op period, the FMS will autonmatically abort (stop) execution of the program.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
int scaleJostick(int value)
{
   if(value>-10 && value<10)
   {
      return 0;
   }
   return value;
}

task main()
{
  initializeRobot();

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

  while (true)
  {
  motor(leftMotor)=scaleJostick(joystick.joy1_y1);
  motor(rightMotor)=scaleJostick(joystick.joy1_y2);

  servo(shoulder)=scaleJostick(joystick.joy2_y1);
  motor(elbow)=scaleJostick(joystick.joy2_y2);


  if (joy1Btn(1) == true)
  {
     servo(wrist)=servo(wrist)+2;
  }

 

     //do something
  if (joy1Btn(2) == true)
  {
     servo(wrist)= servo(wrist)-2;
  }


 



  if (joy1Btn(5) == true)
  {
     motor(slider)= motor(slider)+2;
  }
 
  else
{
   motor(slider)= motor(slider)+0;
}


  if (joy1Btn(6) == true)
  {
     motor(slider)= motor(slider)-2;
  }
 
  else
  {
     motor(slider) = motor(slider)+0;
  }






     ///////////////////////////////////////////////////////////
     ///////////////////////////////////////////////////////////
     ////                                                   ////
     ////      Add your robot specific tele-op code here.   ////
     ////                                                   ////
     ///////////////////////////////////////////////////////////
     ///////////////////////////////////////////////////////////

    // Insert code to have servos and motors respond to joystick and button values.

    // Look in the ROBOTC samples folder for programs that may be similar to what you want to perform.
    // You may be able to find "snippets" of code that are similar to the functions that you want to
    // perform.
  }
}


Please excuse the mess ;)
I only have one servo using the joystick while the rest move on buttons. one motor, called the slider, is working but doesn't stop when I let go of the button after I have pressed it. Also the servos were doing the same thing except now they just plain don't respond. Now there is nothing wrong with any of the servos in case you are wondering, I checked them, I am more concerned with getting them to work and then getting the to move when I press a button and stop when I release the button.

Please help our newbie teem with our problems.
Thank You!

from;
The Technomancers!


Attachments:
TeleOp6958.c [5.31 KiB]
Downloaded 205 times
Thu Oct 31, 2013 1:55 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Servos not responding
From looking at your code, it seems you don't quite understand the difference between a motor and a servo. A motor can take a value between -100 to 100. Negative values will make the motor spin in reverse. The magnitude of the number represents the "% power" you feed the motor. So the bigger the magnitude, the faster it spins. For servo, it takes a value between 0 and 255. For your 180-degree servo, a value zero will make the servo rotate all the way anti-clockwise and hold there. A value of 255 will make the servo rotate all the way clockwise and hold there. So a value of 127 is the "neutral point" or the "center point". So for a servo, once it reaches the "angle" corresponding to the value, it will hold there and not move. So for example:
Code:
 servo(shoulder)=scaleJostick(joystick.joy2_y1);

Your code above is wrong in several aspects. First, servo does not take negative values. So when the Y1 stick is pulled back, it will give you a value of -128 which is actually 128 in unsigned char which means the servo will go to the neutral position and stay there. When the joystick is in neutral, it gives you a zero value. That will actually turn the servo all the way to the left and stay there. If you push the joystick up, it gives you a value 127. Again, it will make the servo go to the neutral position. In other words, if the joystick starts in the neutral position, the servo will start at the left-most position. As the joystick is pushed up, the servo will move from the left most position to the center position. If the joystick is returned to neutral, the servo will go back to the left most position. As you pull the joystick backward, the servo all the sudden jumps to the right most position and as the joystick is pull back more, it will turn towards the neutral position.
If I understand your code correctly. You really want the shoulder be turning and holding its position according to a button press instead of a joystick. For example:
Code:
#define SHOULDER_INC       5
int shoulderPos = 0;
while (true)
{
    if (joy1Btn(1))
    {
        shoulderPos += SHOULDER_INC;
        if (shoulderPos > 255)
        {
            shoulderPos = 255;
        }
    }
    else if (joy1Btn(2))
    {
        shoulderPos -= SHOULDER_INC;
        if (shoulderPos < 0)
        {
            shoulderPos = 0;
        }
    }
    servo[shoulder] = shoulderPos;
    wait1Msec(50);
}

Since I don't really know the design of your robot, it is harder for me to tell what you are trying to do with the "elbow". Elbow is on a Tetrix motor, the way your code works is that the elbow turning speed is proportional to your joystick position. So it will turn faster if you push the joystick more. When you let go the joystick, the elbow motor will stop. However, since it is not a servo, it won't hold the position. So if the elbow is lifted against gravity, it will drop as soon as the joystick is released unless the elbow joint is constructed with jack screws or worm gear such that even the motor is losing power, the gear mechanism will hold the position for you.
Regarding the wrist, your code has a problem. What if the driver presses both button 1 and 2 simultaneously. So for the wrist, you should use the shoulder code I show you above.
For the slider, your code is treating it as a servo which is wrong. The slider is a Tetrix motor, so when button 6 is pressed, you kept increasing the power of the motor. So the longer you pressed button 6, the faster it will slide out. When button 6 is released, it doesn't stop. It will still continue with the last speed you have until the slide is destroying itself.
Does it make sense now?


Thu Oct 31, 2013 5:33 pm
Profile
Rookie

Joined: Sat Oct 26, 2013 11:02 pm
Posts: 8
Post Re: Servos not responding
No no that's not it. The shoulder, which is the very start of the arm, is supposed to work for the joystick. The elbow is another joint which is used by a motor which works just fine( other than the fact that it moves too fast). the wrist is the part that moves the scooper up and down whether to adjust to the floor or dump blocks into a basket. The wrist I want working for the buttons. The first assigned servo, aka shoulder, is assigned to a joystick which will not respond. Every other servo or motor that is assigned to buttons will either not work or move continuously until I shut down the program.


Fri Nov 01, 2013 3:25 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Servos not responding
deathbewithyou wrote:
No no that's not it. The shoulder, which is the very start of the arm, is supposed to work for the joystick. The elbow is another joint which is used by a motor which works just fine( other than the fact that it moves too fast). the wrist is the part that moves the scooper up and down whether to adjust to the floor or dump blocks into a basket. The wrist I want working for the buttons. The first assigned servo, aka shoulder, is assigned to a joystick which will not respond. Every other servo or motor that is assigned to buttons will either not work or move continuously until I shut down the program.

Regarding the shoulder, the question to you is: what is the relationship between the joystick position and the shoulder? Does the joystick position determine the angle of the shoulder? Or does it determine the speed of the shoulder moving? That's very important for deciding how to implement the correct behavior.
Regarding the elbow, you can slow it down by dividing the joystick value by a factor or 2 or even 3.
Code:
  motor[elbow]=scaleJostick(joystick.joy2_y2)/2;

PS, motor is an array variable. So the proper way to access it is using the "square brackets" not the "round brackets". I don't know why the RobotC folks decided to allow them to be used interchangeably. This will promote incorrect learning. There is a View->Preferences->Detail preferences->Compiler option to turn this off. motor() makes it look like a function call. And it is totally ILLEGAL to put a function call on the left side of an assignment operator. If you get into a habit of accessing array with the round brackets, your code is not portable to platforms other than RobotC. Just be warned.

Regarding the other servo problems, like I said, you are programming them as if they were Tetrix motors. That won't work. Please read the explanation in my previous post carefully. The main issue of your wrist code is that you need to add a delay in your main loop. For example, you are incrementing or decrementing the wrist servo by 2 when button one or two is pressed. Keep in mind that NXT can run your code really fast, so in a split second, it will already be incrementing the value way pass 255. So you need to do two things:
1. Add a "wait1Msec(100)" at the end of your loop.
2. After incrementing or decrementing the wrist servo value, you need to check if the value has exceeded 255. If so, you need to cap it at 255. Similarly, if you have decremented the servo value below zero, you need to cap it at zero. Like I said before, the servo can only take a value between 0 and 255.


Fri Nov 01, 2013 5:18 pm
Profile
Rookie

Joined: Sat Oct 26, 2013 11:02 pm
Posts: 8
Post Re: Servos not responding
The shoulder is the part that can turn the arm left and right so that my arm will have more maneuverability. For some reason it won't work on my joystick like it did before. Also if I use the program you gave me on the first post, will it allow the servo to move a little bit or all the way to 255 as I press btn 1? I don't want it going that far, just a certain amount.

I've also updated my file a bit and I need to know if I am on the right track. I'm also trying to assign an infrared seeker on robotC but I can't seem to find it on motors and sensors set up.

#pragma config(Hubs, S1, HTMotor, HTMotor, HTServo, none)
#pragma config(Motor, mtr_S1_C1_1, leftMotor, tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor, mtr_S1_C1_2, rightMotor, tmotorTetrix, openLoop, encoder)
#pragma config(Motor, mtr_S1_C2_1, elbow, tmotorTetrix, openLoop)
#pragma config(Motor, mtr_S1_C2_2, slider, tmotorTetrix, openLoop)
#pragma config(Servo, srvo_S1_C3_1, shoulder, tServoStandard)
#pragma config(Servo, srvo_S1_C3_2, wrist, tServoStandard)
#pragma config(Servo, srvo_S1_C3_3, flag, tServoContinuousRotation)
#pragma config(Servo, srvo_S1_C3_4, servo4, tServoNone)
#pragma config(Servo, srvo_S1_C3_5, servo5, tServoNone)
#pragma config(Servo, srvo_S1_C3_6, servo6, tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
// Tele-Operation Mode Code Template
//
// This file contains a template for simplified creation of an tele-op program for an FTC
// competition.
//
// You need to customize two functions with code unique to your specific robot.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

#include "JoystickDriver.c" //Include file to "handle" the Bluetooth messages.


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
// initializeRobot
//
// Prior to the start of tele-op mode, you may want to perform some initialization on your robot
// and the variables within your program.
//
// In most cases, you may not have to add any code to this function and it will remain "empty".
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

void initializeRobot()
{
if(ServoValue[servo1] < 128) // If servo1 is closer to 0 (than 255):
{
while(ServoValue[servo1] < 255) // While the ServoValue of servo1 is less than 255:
{
servo[servo1] = 128; // Move servo1 to position to 255.
}
}
// Place code here to sinitialize servos to starting positions.
// Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.

return;
}


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
// Main Task
//
// The following is the main code for the tele-op robot operation. Customize as appropriate for
// your specific robot.
//
// Game controller / joystick information is sent periodically (about every 50 milliseconds) from
// the FMS (Field Management System) to the robot. Most tele-op programs will follow the following
// logic:
// 1. Loop forever repeating the following actions:
// 2. Get the latest game controller / joystick settings that have been received from the PC.
// 3. Perform appropriate actions based on the joystick + buttons settings. This is usually a
// simple action:
// * Joystick values are usually directly translated into power levels for a motor or
// position of a servo.
// * Buttons are usually used to start/stop a motor or cause a servo to move to a specific
// position.
// 4. Repeat the loop.
//
// Your program needs to continuously loop because you need to continuously respond to changes in
// the game controller settings.
//
// At the end of the tele-op period, the FMS will autonmatically abort (stop) execution of the program.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
int scaleJostick(int value)
{
if(value>-10 && value<10)
{
return 0;
}
return value;
}

task main()
{
initializeRobot();

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

while (true)
{
motor[leftMotor]=scaleJostick(joystick.joy1_y1);
motor[rightMotor]=scaleJostick(joystick.joy1_y2);

servo[shoulder]=scaleJostick(joystick.joy2_y1);
motor[elbow]=scaleJostick(joystick.joy2_y2)/2;



}
#define wrist_INC 5
int wristPos = 0;
while (true)
{
if (joy1Btn(1))
{
wristPos += wrist_INC;
if (wristPos > 255)
{
wristPos = 255;
}
}
else if (joy1Btn(2))
{
wristPos -= wrist_INC;
if (wristPos < 0)
{
wristPos = 0;
}
}
servo[wrist] = wristPos;
wait1Msec(100);
}


//do something





if (joy1Btn(5) == true)
{
motor(slider)= motor(slider)+2;
}

else
{
motor(slider)= motor(slider)+0;
}


if (joy1Btn(6) == true)
{
motor(slider)= motor(slider)-2;
}

else
{
motor(slider) = motor(slider)+0;
}






///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////
//// ////
//// Add your robot specific tele-op code here. ////
//// ////
///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////

// Insert code to have servos and motors respond to joystick and button values.

// Look in the ROBOTC samples folder for programs that may be similar to what you want to perform.
// You may be able to find "snippets" of code that are similar to the functions that you want to
// perform.
}

Sorry for the inconvenience.


Sat Nov 02, 2013 8:00 pm
Profile
Rookie

Joined: Sat Oct 26, 2013 11:02 pm
Posts: 8
Post Re: Servos not responding
sorry, I forgot to put code. Please excuse that.
Code:
#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Motor,  mtr_S1_C1_1,     leftMotor,     tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     rightMotor,    tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     elbow,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     slider,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C3_1,    shoulder,             tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    wrist,                tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    flag,                 tServoContinuousRotation)
#pragma config(Servo,  srvo_S1_C3_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                           Tele-Operation Mode Code Template
//
// This file contains a template for simplified creation of an tele-op program for an FTC
// competition.
//
// You need to customize two functions with code unique to your specific robot.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                    initializeRobot
//
// Prior to the start of tele-op mode, you may want to perform some initialization on your robot
// and the variables within your program.
//
// In most cases, you may not have to add any code to this function and it will remain "empty".
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

void initializeRobot()
{
   if(ServoValue[servo1] < 128)              // If servo1 is closer to 0 (than 255):
    {
      while(ServoValue[servo1] < 255)               // While the ServoValue of servo1 is less than 255:
      {
        servo[servo1] = 128;                              // Move servo1 to position to 255.
      }
    }
  // Place code here to sinitialize servos to starting positions.
  // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.

  return;
}


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                         Main Task
//
// The following is the main code for the tele-op robot operation. Customize as appropriate for
// your specific robot.
//
// Game controller / joystick information is sent periodically (about every 50 milliseconds) from
// the FMS (Field Management System) to the robot. Most tele-op programs will follow the following
// logic:
//   1. Loop forever repeating the following actions:
//   2. Get the latest game controller / joystick settings that have been received from the PC.
//   3. Perform appropriate actions based on the joystick + buttons settings. This is usually a
//      simple action:
//      *  Joystick values are usually directly translated into power levels for a motor or
//         position of a servo.
//      *  Buttons are usually used to start/stop a motor or cause a servo to move to a specific
//         position.
//   4. Repeat the loop.
//
// Your program needs to continuously loop because you need to continuously respond to changes in
// the game controller settings.
//
// At the end of the tele-op period, the FMS will autonmatically abort (stop) execution of the program.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
int scaleJostick(int value)
{
   if(value>-10 && value<10)
   {
      return 0;
   }
   return value;
}

task main()
{
  initializeRobot();

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

  while (true)
  {
  motor[leftMotor]=scaleJostick(joystick.joy1_y1);
  motor[rightMotor]=scaleJostick(joystick.joy1_y2);

  servo[shoulder]=scaleJostick(joystick.joy2_y1);
  motor[elbow]=scaleJostick(joystick.joy2_y2)/2;


 
  }
#define wrist_INC       5
int wristPos = 0;
while (true)
{
    if (joy1Btn(1))
    {
        wristPos += wrist_INC;
        if (wristPos > 255)
        {
            wristPos = 255;
        }
    }
    else if (joy1Btn(2))
    {
        wristPos -= wrist_INC;
        if (wristPos < 0)
        {
            wristPos = 0;
        }
    }
    servo[wrist] = wristPos;
    wait1Msec(100);
}
 

     //do something
 
 



  if (joy1Btn(5) == true)
  {
     motor(slider)= motor(slider)+2;
  }
 
  else
{
   motor(slider)= motor(slider)+0;
}


  if (joy1Btn(6) == true)
  {
     motor(slider)= motor(slider)-2;
  }
 
  else
  {
     motor(slider) = motor(slider)+0;
  }






     ///////////////////////////////////////////////////////////
     ///////////////////////////////////////////////////////////
     ////                                                   ////
     ////      Add your robot specific tele-op code here.   ////
     ////                                                   ////
     ///////////////////////////////////////////////////////////
     ///////////////////////////////////////////////////////////

    // Insert code to have servos and motors respond to joystick and button values.

    // Look in the ROBOTC samples folder for programs that may be similar to what you want to perform.
    // You may be able to find "snippets" of code that are similar to the functions that you want to
    // perform.
  }


Sat Nov 02, 2013 8:15 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Servos not responding
deathbewithyou wrote:
The shoulder is the part that can turn the arm left and right so that my arm will have more maneuverability. For some reason it won't work on my joystick like it did before. Also if I use the program you gave me on the first post, will it allow the servo to move a little bit or all the way to 255 as I press btn 1? I don't want it going that far, just a certain amount.

I've also updated my file a bit and I need to know if I am on the right track. I'm also trying to assign an infrared seeker on robotC but I can't seem to find it on motors and sensors set up.

Like I said before, you cannot use joystick to control the speed of a standard 180-degree servo. You cannot control the speed of a standard servo, period. The only thing you can control on a standard servo is the position. So yes the code I posted will step the servo at SHOULDER_INC for each loop. Since each loop has a 50msec delay, with a SHOULDER_INC of 5, stepping from 0 to 255 requires 51 loops. So the total time it will take to step from 0 to 255 is 51*50 msec = 2550 msec = 2.55 second. In other words, when you press and hold button 1, it will take approx. 2.5 seconds to swing the shoulder from one end to the other. If you want it go faster, you increase the SHOULDER_INC value. If you want slower, decrease it.
Regarding you new code, it has a few problems. First, you have two infinite loops one after the other. By definition, infinite loop never exits. So the second infinite loop will never execute.
Code:
task main()
{
  initializeRobot();

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

  while (true)
  {
    motor[leftMotor]=scaleJostick(joystick.joy1_y1);
    motor[rightMotor]=scaleJostick(joystick.joy1_y2);

    servo[shoulder]=scaleJostick(joystick.joy2_y1);
    motor[elbow]=scaleJostick(joystick.joy2_y2)/2;
  }
//
// MHTS: You will never reach the code below!!!
//
#define wrist_INC       5
int wristPos = 0;
while (true)
{

I cleaned up your code and added some comments.
Code:
#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Motor,  mtr_S1_C1_1,     leftMotor,     tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     rightMotor,    tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     elbow,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     slider,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C3_1,    shoulder,             tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    wrist,                tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    flag,                 tServoContinuousRotation)
#pragma config(Servo,  srvo_S1_C3_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                           Tele-Operation Mode Code Template
//
// This file contains a template for simplified creation of an tele-op program for an FTC
// competition.
//
// You need to customize two functions with code unique to your specific robot.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                    initializeRobot
//
// Prior to the start of tele-op mode, you may want to perform some initialization on your robot
// and the variables within your program.
//
// In most cases, you may not have to add any code to this function and it will remain "empty".
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

void initializeRobot()
{
  //
  // MHTS: I don't understand what you are trying to do here?
  //       Are you trying to set servo1 to 128 and waiting for it
  //       to go there? If so, don't bother. ServoValue[] only
  //       returns what you set the servo to. It doesn't tell you
  //       the actual position the servo is at. So the following
  //       code has no effect. Get rid of it.
  //
  if(ServoValue[servo1] < 128)              // If servo1 is closer to 0 (than 255):
  {
    while(ServoValue[servo1] < 255)               // While the ServoValue of servo1 is less than 255:
    {
      servo[servo1] = 128;                              // Move servo1 to position to 255.
    }
  }
  // Place code here to sinitialize servos to starting positions.
  // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.

  return;
}


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                         Main Task
//
// The following is the main code for the tele-op robot operation. Customize as appropriate for
// your specific robot.
//
// Game controller / joystick information is sent periodically (about every 50 milliseconds) from
// the FMS (Field Management System) to the robot. Most tele-op programs will follow the following
// logic:
//   1. Loop forever repeating the following actions:
//   2. Get the latest game controller / joystick settings that have been received from the PC.
//   3. Perform appropriate actions based on the joystick + buttons settings. This is usually a
//      simple action:
//      *  Joystick values are usually directly translated into power levels for a motor or
//         position of a servo.
//      *  Buttons are usually used to start/stop a motor or cause a servo to move to a specific
//         position.
//   4. Repeat the loop.
//
// Your program needs to continuously loop because you need to continuously respond to changes in
// the game controller settings.
//
// At the end of the tele-op period, the FMS will autonmatically abort (stop) execution of the program.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

#define DEAD_BAND(n, d) ((abs(n) < (d))? 0: (n))

#define wrist_INC       5
#define shoulder_INC    5

int wristPos = 0;
int shoulderPos = 0;

task main()
{
  initializeRobot();

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

  while (true)
  {
    motor[leftMotor] = DEAD_BAND(joystick.joy1_y1, 10);
    motor[rightMotor] = DEAD_BAND(joystick.joy1_y2, 10);
    motor[elbow] = DEAD_BAND(joystick.joy2_y2, 10)/2;
    //
    // MHTS: Since slider is on a Tetrix motor, it can be
    //       controlled by the joystick, no need to use
    //       buttons.
    //
    motor[slider] = DEAD_BAND(joystick.joy2_y1, 10)/2;

    if (joy1Btn(3))
    {
      shoulderPos += shoulder_INC;
      if (shoulderPos > 255)
      {
        shoulderPos = 255;
      }
    }
    else if (joy1Btn(4))
    {
      shoulderPos -= shoulder_INC;
      if (shoulderPos < 0)
      {
        shoulderPos = 0;
      }
    }
    servo[shoulder] = shoulderPos;

    //
    // MHTS: The following code is moved up from the next infinite
    //       loop since it will never be executed down there.
    //
    if (joy1Btn(1))
    {
      wristPos += wrist_INC;
      if (wristPos > 255)
      {
        wristPos = 255;
      }
    }
    else if (joy1Btn(2))
    {
      wristPos -= wrist_INC;
      if (wristPos < 0)
      {
        wristPos = 0;
      }
    }
    servo[wrist] = wristPos;

    wait1Msec(50);
  }
}


Sat Nov 02, 2013 11:56 pm
Profile
Rookie

Joined: Sat Oct 26, 2013 11:02 pm
Posts: 8
Post Re: Servos not responding
now hold on a minute! the slider is more of a secondary measure. I'm only going to use the slider once. Wouldn't it be easier to have my servos set to the joysticks? All servos, except for the servo made for the flag, are going to be used to grab the blocks.


Sun Nov 03, 2013 10:58 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Servos not responding
deathbewithyou wrote:
now hold on a minute! the slider is more of a secondary measure. I'm only going to use the slider once. Wouldn't it be easier to have my servos set to the joysticks? All servos, except for the servo made for the flag, are going to be used to grab the blocks.

That depends on what you meant by "to have the servos set to the joysticks"? For example, what is the expected behavior when you push the joystick all the way up, half way up, leave at neutral, half way down, or all the way down? If you can answer these questions, I will tell you if it is possible to do it or not.


Sun Nov 03, 2013 11:30 pm
Profile
Rookie

Joined: Sat Oct 26, 2013 11:02 pm
Posts: 8
Post Re: Servos not responding
I really do not know. But It would be easier to control the servos with the joysticks. 2 of the servos function for the arm and I need to use those for the competition and the qualifier is coming this Saturday. My team is really in a bind here.


Tue Nov 05, 2013 9:33 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Servos not responding
But what do you want it to behave? If it is a servo, you can't really control its "speed" directly. So having the joystick to control it seems pointless. If you insist to use the joystick, the only thing you can do is to treat it like a button. Push the joystick up beyond deadband will be like pressing the up button and pull the joystick down beyond deadband will be like pressing the down button. How much you push the joystick is then irrelevant. Having said that, it is possible to simulate speed on a servo and use the joystick to control it. For example:
Code:
task main()
{
    int shoulderPos = 0;
    while (true)
    {
        getJoystickSettings(joystick);
        int shoulderStep = scaleJoystick(joystick.joy2_y1)/10;
        shoulderPos += shouderStep;
        if (shoulderPos > 255)
        {
            shoulderPos = 255;
        }
        else if (shoulderPos < 0)
        {
            shoulderPos = 0;
        }
        servo[shoulder] = shoulderPos;
        wait1Msec(50);
    }
}


Wed Nov 06, 2013 12:24 am
Profile
Rookie

Joined: Sat Oct 26, 2013 11:02 pm
Posts: 8
Post Re: Servos not responding
Hello, I've been gone for a while, but now I am back. Still having some trouble with my program. I've had to change things a bit and I also need help with an autonomous program that I don't know if the infrared scanner is working and how to work the sonar sensor properly to keep my robot from crashing into the beacon. Still need help with the servos, The servo called wrist will not respond to commands for the button. I also need help with the continuous rotation servo motor, I don't know how to program that. Please, do not focus on the shoulder! We have removed that from the robot, anyone working on that will be ignored! What I need here is working servos and to work my autonomous program. Sorry for the trouble.

Code:
pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S2,     seeker,         sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S3,     sonarSensor,    sensorSONAR)
#pragma config(Motor,  mtr_S1_C1_1,     leftMotor,     tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     rightMotor,    tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     elbow,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     slider,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C3_1,    shoulder,             tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    wrist,                tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_6,    flag,                 tServoContinuousRotation)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//




#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.




void initializeRobot()
{
   if(ServoValue[servo1] < 128)              // If servo1 is closer to 0 (than 255):
    {
      while(ServoValue[servo1] < 255)               // While the ServoValue of servo1 is less than 255:
      {
        servo[servo1] = 128;                              // Move servo1 to position to 255.
      }
    }
  .

  return;
}



int scaleJostick(int value)
{
   if(value>-10 && value<10)
   {
      return 0;
   }
   return value;
}


task main()
{
   int max_power=50;
   int min_power=10;
   int side_left;
   int side_right;
   int wall= 100;


  initializeRobot();

  waitForStart();   // wait for start of tele-op phase
  while (SensorValue[sonarSensor] > wall) // I did some tweaks on this but I still need advice for the sonar sensor
  {

while (true)
{
   int reading= SensorValue[seeker];
   side_right=-(reading*min_power)+(min_power+max_power);
   side_left=(reading*min_power)-(max_power-min_power);


// We don't know if this part of the program works since we don't have a beacon. However we do know that the sonar sensor is not functioning.
   

   if(reading<5)
   {
   motor[rightMotor]=side_right;
   motor[leftMotor]=side_right;
}

else if(reading>5)
{
   motor[rightMotor]=side_left;
   motor[leftMotor]=side_left;
}

else if(reading==5)
{
   motor[rightMotor]=3*min_power;
   motor[leftMotor]=3*min_power;
}
}
motor[rightMotor]=0;
motor[leftMotor]=0;
servo[wrist]= 20;// This may or may not work. It is imperrative that I get this working.

motor[elbow]=10;
wait1Msec(1000);

}
///////////////////////////////
// please ignore below...//////
///////////////////////////////

  while (true)
  {
 motor[elbow]=scaleJostick(joystick.joy2_y2/5);

  motor[leftMotor]=scaleJostick(joystick.joy1_y1);
  motor[rightMotor]=scaleJostick(joystick.joy1_y2);
  motor[slider]=scaleJostick(joystick.joy2_y1);








  }


while(joy1Btn(1)==true)
{
   servo[wrist]=30;//don't work...

}


while (joy1Btn(2)==true)
{

servo[wrist]=80;// nada!
}


  while (joy1Btn(3) == true)
  {
servo[wrist]=113;//Nope!

  }




  while (joy1Btn(4) == true)
  {
servo[wrist]=185; // I still can't get it to work.
  }









  }


and now the code for tele op phase:

Code:
#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     seeker,         sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S3,     sonarSensor,    sensorSONAR)
#pragma config(Motor,  mtr_S1_C1_1,     leftMotor,     tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     rightMotor,    tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     elbow,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     slider,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C3_1,    wrist,                tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_6,    flag,                 tServoContinuousRotation)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//



#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.




void initializeRobot()
{
   servoTarget[wrist]=ServoValue[wrist] +1;


  // Place code here to sinitialize servos to starting positions.
  // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.

  return;
}



int scaleJostick(int value)
{
   if(value>-10 && value<10)
   {
      return 0;
   }
   return value;
}


task main()
{











  while (true)
  {
 motor[elbow]=scaleJostick(joystick.joy2_y2/5); // motors work perfectly!

  motor[leftMotor]=scaleJostick(joystick.joy1_y1);
  motor[rightMotor]=scaleJostick(joystick.joy1_y2);
  motor[slider]=scaleJostick(joystick.joy2_y1/2 );








  }
//The servos will not obey my commands. There have been many times when I wanted to grab a shotgun and blow my laptop into smithereans, but that would be
// costly and would ruin my team(and by far the parents') mood. At any rate, I have attempted to make changes, none worked so I need a solution by tomorrow
// night. Thank you for your help. 

while(joy2Btn(1)==true)
{
   servoChangeRate[wrist]=3;
   servo[wrist]=30;


}


while (joy2Btn(2)==true)
{
servoChangeRate[wrist]=3;
servo[wrist]=80;

}


  while (joy2Btn(3) == true)
  {

servo[wrist]=113;
  }




  while (joy2Btn(4) == true)
  {
servo[wrist]=185;
  }









  }


Excuse the shotgun comment please.


Thu Jan 09, 2014 11:47 pm
Profile
Expert
User avatar

Joined: Sat Aug 31, 2013 9:15 am
Posts: 101
Post Re: Servos not responding
Your problem for your autonomous program is your loops. You have multiple infinite loops in your program, so only the first one executes. In your autonomous program, you want to eliminate all infinite loops except for an empty one at the very end of the program that keeps the program from ending.

For your driver control program, your problem is again multiple loops. For this program, you want to have one infinite loop that all of your joystick code is inside.

_________________
Burning Lights Programming
FTC Team 6100 Chariots of Fire - Programmer (2012-2013)
FTC Team 7468 Blue Chariots of Fire - Programmer (2013-2014)
Check out our team website at http://cof7468.weebly.com/.


Fri Jan 10, 2014 12:45 pm
Profile
Rookie

Joined: Sat Oct 26, 2013 11:02 pm
Posts: 8
Post Re: Servos not responding
Well I do not know about eliminating the sonar sensor if that is what you mean. I need to somehow work the sonar so it does not crash into anything, especially when it could tear through my mom's chair. Most importantly I need to use the sonar to tell the robot when to stop and drop a block into a basket.
As for my servos, I appear to have solved the problem for my 180 degree servo. However, I still have problems with the continuous rotation servo.

Code:

#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     seeker,         sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S3,     sonarSensor,    sensorSONAR)
#pragma config(Motor,  mtr_S1_C1_1,     leftMotor,     tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     rightMotor,    tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     elbow,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     slider,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C3_1,    wrist,                tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_6,    flag,                 tServoContinuousRotation)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//



#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.

int wristPosition;
int flagPosition;


void initializeRobot()
{
   servo[wrist] = 0;
   wait1Msec(1000);

   return;


  // Place code here to sinitialize servos to starting positions.
  // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.


}

void wristUp()
{
   wristPosition = wristPosition +1;
   if (wristPosition > 255)
   {
      wristPosition = 255;
   }
   servo[wrist]= wristPosition;
}

void wristDown()
{
   wristPosition = wristPosition - 1;
   if (wristPosition < 0)
   {
      wristPosition = 0;
   }
   servo[wrist] = wristPosition;
}

void clockWise()
{
   flagPosition = flagPosition + 1;
   if (flagPosition > 255)
   {
      flagPosition = 255;
   }
   servo[flag]= flagPosition;
}

void stopflag()
{
   flagPosition = flagPosition + 0;
   if (flagPosition == 127)
   {
      flagPosition = 127;
   }
   servo[flag] = flagPosition;
}

int scaleJostick(int value)
{
   if(value>-10 && value<10)
   {
      return 0;
   }
   return value;
}


task main()
{
initializeRobot();

waitForStart();








  while (true)
  {
 motor[elbow]=scaleJostick(joystick.joy2_y2/5); // motors work perfectly!

  motor[leftMotor]=scaleJostick(joystick.joy1_y1);
  motor[rightMotor]=scaleJostick(joystick.joy1_y2);
  motor[slider]=scaleJostick(joystick.joy2_y1/2 );


  if (joy2Btn(2))
  {
     wristDown(); // wrote a given code that seemed to work pretty well and am happy for the outcome!
     wait1Msec(5);
  }
 
  if (joy2Btn(1))
  {
     wristUp();
     wait1Msec(5);
  }
 
  if (joy2Btn(4))
  {
     
     clockWise(); // tried the samthing for the continuous rotation servo. However they do not appear to be the same...
     
     
  }
 
  if (joy2Btn(3))
  {   
     
  stopflag();
 
     
}

}



  }


Of course, since I have fixed one of the problems for my joystick controls, I really need to focus on the autonomous program. Any given information will be appreciated.


Fri Jan 10, 2014 2:33 pm
Profile
Expert
User avatar

Joined: Sat Aug 31, 2013 9:15 am
Posts: 101
Post Re: Servos not responding
Is the problem that your continuous rotation servo doesn't stop? If so, I think your problem is in a small typo. Your code reads:
Code:
if (flagPosition == 127)
   {
      flagPosition = 127;
   }


Which does absolutely nothing to change the value of flagPosition. If flagPosition is not 127, then it will keep its value. If it is 127, it will be set to 127, which still does nothing to change its value. I think what you may mean is:
Code:
if (flagPosition != 127)
   {
      flagPosition = 127;
   }


And for the autonomous program, I was not referring to removing the ultrasonic sensor. Look in your autonomous program for the first time that it says while (true). Look at what the brackets for that while loop enclose. The code that is inside those brackets will be executed over and over forever, and anything after that loop will never execute.

_________________
Burning Lights Programming
FTC Team 6100 Chariots of Fire - Programmer (2012-2013)
FTC Team 7468 Blue Chariots of Fire - Programmer (2013-2014)
Check out our team website at http://cof7468.weebly.com/.


Fri Jan 10, 2014 9:58 pm
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 15 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.