View unanswered posts | View active topics It is currently Wed Sep 03, 2014 12:48 am






Reply to topic  [ 2 posts ] 
Motor Jerking 
Author Message
Rookie

Joined: Sun Jan 20, 2013 7:59 pm
Posts: 1
Post Motor Jerking
Ok i'm kinda new to this so I could use a little help PLEASE!!
In Tank Drive our accessory motors for our lift and our arm jerk in both directions.
This is motor H and I can some one look over and make sure Im not missing something. Also put a volt meter on the controller brick getting about 9v in one direction 3.2v on the other. If motor plugged to 12v runs great in either direction.


#pragma config(Hubs, S1, HTMotor, HTMotor, none, none)
#pragma config(Hubs, S4, HTMotor, none, none, none)
#pragma config(Sensor, S1, , sensorI2CMuxController)
#pragma config(Sensor, S4, , sensorI2CMuxController)
#pragma config(Motor, mtr_S1_C1_1, motorD, tmotorTetrix, openLoop)
#pragma config(Motor, mtr_S1_C1_2, motorE, tmotorTetrix, openLoop, reversed)
#pragma config(Motor, mtr_S1_C2_1, motorF, tmotorTetrix, openLoop, reversed)
#pragma config(Motor, mtr_S1_C2_2, motorG, tmotorTetrix, openLoop)
#pragma config(Motor, mtr_S4_C1_1, motorH, tmotorTetrix, PIDControl)
#pragma config(Motor, mtr_S4_C1_2, motorI, tmotorTetrix, PIDControl)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//

/*--------------------------------------------------------------------------------------------------------*\
|* *|
|* - Tetrix Quiet Tank Drive - *|
|* ROBOTC on Tetrix *|
|* *|
|* This program allows you to drive a robot via remote control using the ROBOTC Debugger. *|
|* This particular method uses "Tank Drive" where each side is controlled individually like a tank. *|
|* This program also ignores low values that would cause your robot to move when the joysticks fail to *|
|* return back to exact center. You may need to play with the 'threshold' value to get it just right. *|
|* *|
|* ROBOT CONFIGURATION *|
|* NOTES: *|
|* *|
|* MOTORS & SENSORS: *|
|* [I/O Port] [Name] [Type] [Description] *|
|* Port D motorD 12V Right motor *|
|* Port E motorE 12V Left motor *|
\*---------------------------------------------------------------------------------------------------4246-*/

#include "JoystickDriver.c"

task main()
{
int threshold = 10; /* Int 'threshold' will allow us to ignore low */
int counter = 3;
float CR=0.00;
int Forward = 1;
int Forward2= 1;
int ScoopPosition = 2; //MM :D add this
int FlipPosition = 2; //MM :D add this

while(true) // Infinite loop:
{
getJoystickSettings(joystick);

counter = 3;
CR = counter/2;

if(abs(joystick.joy1_y1) > threshold*Forward) // If the right analog stick's Y-axis readings are either above or below the threshold:
{
motor[motorE] = joystick.joy1_y1 * Forward; // Motor D is assigned a power level equal to the right analog stick's Y-axis reading.
}
else // Else if the readings are within the threshold:
{
if(joystick.joy1_TopHat == 4) // if the topmost button on joy1's D-Pad ('TopHat') is pressed:
{
motor[motorE] = -10; // motorE is run at a power level of 50
//motor[motorD] = -10; // motorD is run at a power level of 50
}
else if(joystick.joy1_TopHat == 0)
{
motor(motorE)=20;
//motor(motorD)=20;
}
else motor[motorE] = 0; // Motor D is stopped with a power level of 0.
}


if(abs(joystick.joy1_y2) > threshold * Forward) // If the left analog stick's Y-axis readings are either above or below the threshold:
{
motor[motorD] = joystick.joy1_y2; // Motor E is assigned a power level equal to the left analog stick's Y-axis reading.
}
else // Else if the readings are within the threshold:
{
if(joystick.joy1_TopHat == 4) // if the topmost button on joy1's D-Pad ('TopHat') is pressed:
{
//motor[motorE] = -10; // motorE is run at a power level of 50
motor[motorD] = -10; // motorD is run at a power level of 50
}
else if(joystick.joy1_TopHat == 0)
{
//motor(motorE)=20;
motor(motorD)=20;
}
else motor[motorD] = 0; // Motor E is stopped with a power level of 0.
}

if(abs(joystick.joy1_y2) > threshold*Forward2) // If the right analog stick's Y-axis readings are either above or below the threshold:
{ motor[motorF] = joystick.joy2_y2; // Motor E is assigned a power level equal to the left analog stick's Y-axis reading.
}
else // Else if the readings are within the threshold:
{
motor[motorF]= 0; // Motor E is stopped with a power level of 0.
}


if(abs(joystick.joy1_y1) > threshold * Forward2) // If the left analog stick's Y-axis readings are either above or below the threshold:
{
motor[motorG] = joystick.joy2_y1; // Motor E is assigned a power level equal to the left analog stick's Y-axis reading.
}
else // Else if the readings are within the threshold:
{
motor[motorG]= 0; // Motor E is stopped with a power level of 0.
}



nPidUpdateInterval = 20; // a good interval is 20
if(joy1Btn(5))
motor(motorI)=100;

if(joy1Btn(6))
motor(motorI)=-100;

if(joy1Btn(2))
motor(motorH)=-100;


if(joy1Btn(4))
motor(motorH)=100;


}
}


Sun Jan 20, 2013 8:11 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Motor Jerking
I have put your code in a code block so it can preserve the indentation.
Code:
#pragma config(Hubs, S1, HTMotor, HTMotor, none, none)
#pragma config(Hubs, S4, HTMotor, none, none, none)
#pragma config(Sensor, S1, , sensorI2CMuxController)
#pragma config(Sensor, S4, , sensorI2CMuxController)
#pragma config(Motor, mtr_S1_C1_1, motorD, tmotorTetrix, openLoop)
#pragma config(Motor, mtr_S1_C1_2, motorE, tmotorTetrix, openLoop, reversed)
#pragma config(Motor, mtr_S1_C2_1, motorF, tmotorTetrix, openLoop, reversed)
#pragma config(Motor, mtr_S1_C2_2, motorG, tmotorTetrix, openLoop)
#pragma config(Motor, mtr_S4_C1_1, motorH, tmotorTetrix, PIDControl)
#pragma config(Motor, mtr_S4_C1_2, motorI, tmotorTetrix, PIDControl)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//

/*--------------------------------------------------------------------------------------------------------*\
|*                                                                                                        *|
|* - Tetrix Quiet Tank Drive -                                                                            *|
|*   ROBOTC on Tetrix                                                                                     *|
|*                                                                                                        *|
|* This program allows you to drive a robot via remote control using the ROBOTC Debugger.                 *|
|* This particular method uses "Tank Drive" where each side is controlled individually like a tank.       *|
|* This program also ignores low values that would cause your robot to move when the joysticks fail to    *|
|* return back to exact center. You may need to play with the 'threshold' value to get it just right.     *|
|*                                                                                                        *|
|* ROBOT CONFIGURATION                                                                                    *|
|* NOTES:                                                                                                 *|
|*                                                                                                        *|
|* MOTORS & SENSORS:                                                                                      *|
|* [I/O Port] [Name] [Type] [Description]                                                                 *|
|* Port D motorD 12V Right motor                                                                          *|
|* Port E motorE 12V Left motor                                                                           *|
\*---------------------------------------------------------------------------------------------------4246-*/

#include "JoystickDriver.c"

task main()
{
    int threshold = 10; /* Int 'threshold' will allow us to ignore low */
    int counter = 3;
    float CR=0.00;
    int Forward = 1;
    int Forward2= 1;
    int ScoopPosition = 2; //MM :D add this
    int FlipPosition = 2; //MM :D add this

    while(true) // Infinite loop:
    {
        getJoystickSettings(joystick);

        counter = 3;
        CR = counter/2;

        if(abs(joystick.joy1_y1) > threshold*Forward) // If the right analog stick's Y-axis readings are either above or below the threshold:
        {
            motor[motorE] = joystick.joy1_y1 * Forward; // Motor D is assigned a power level equal to the right analog stick's Y-axis reading.
        }
        else // Else if the readings are within the threshold:
        {
            if(joystick.joy1_TopHat == 4) // if the topmost button on joy1's D-Pad ('TopHat') is pressed:
            {
                motor[motorE] = -10; // motorE is run at a power level of 50
                //motor[motorD] = -10; // motorD is run at a power level of 50
            }
            else if(joystick.joy1_TopHat == 0)
            {
                motor(motorE)=20;
                //motor(motorD)=20;
            }
            else motor[motorE] = 0; // Motor D is stopped with a power level of 0.
        }

        if(abs(joystick.joy1_y2) > threshold * Forward) // If the left analog stick's Y-axis readings are either above or below the threshold:
        {
            motor[motorD] = joystick.joy1_y2; // Motor E is assigned a power level equal to the left analog stick's Y-axis reading.
        }
        else // Else if the readings are within the threshold:
        {
            if(joystick.joy1_TopHat == 4) // if the topmost button on joy1's D-Pad ('TopHat') is pressed:
            {
                //motor[motorE] = -10; // motorE is run at a power level of 50
                motor[motorD] = -10; // motorD is run at a power level of 50
            }
            else if(joystick.joy1_TopHat == 0)
            {
                //motor(motorE)=20;
                motor(motorD)=20;
            }
            else motor[motorD] = 0; // Motor E is stopped with a power level of 0.
        }

        if(abs(joystick.joy1_y2) > threshold*Forward2) // If the right analog stick's Y-axis readings are either above or below the threshold:
        {
            motor[motorF] = joystick.joy2_y2; // Motor E is assigned a power level equal to the left analog stick's Y-axis reading.
        }
        else // Else if the readings are within the threshold:
        {
            motor[motorF]= 0; // Motor E is stopped with a power level of 0.
        }

        if(abs(joystick.joy1_y1) > threshold * Forward2) // If the left analog stick's Y-axis readings are either above or below the threshold:
        {
            motor[motorG] = joystick.joy2_y1; // Motor E is assigned a power level equal to the left analog stick's Y-axis reading.
        }
        else // Else if the readings are within the threshold:
        {
            motor[motorG]= 0; // Motor E is stopped with a power level of 0.
        }

        nPidUpdateInterval = 20; // a good interval is 20
        if(joy1Btn(5))
            motor(motorI)=100;

        if(joy1Btn(6))
            motor(motorI)=-100;

        if(joy1Btn(2))
            motor(motorH)=-100;

        if(joy1Btn(4))
            motor(motorH)=100;
    }
}

I can spot many problems in your code. What do you mean by "jerk in both directions"? Ignoring the rest of the program, the code dealing with motor H and I is:
Code:
        if(joy1Btn(5))
            motor(motorI)=100;

        if(joy1Btn(6))
            motor(motorI)=-100;

        if(joy1Btn(2))
            motor(motorH)=-100;

        if(joy1Btn(4))
            motor(motorH)=100;

This code means as soon as button 5 is pressed, motor I is running at full power forward, but as soon as button 6 is pressed, motor I is running full power backward. This is very bad for the motor, motor controller and battery especially if the motor is running full power forward and you press button 6. This will make the motor to run full reverse in a split second. If that's the jerkiness you meant, then that's what you told the motor to do. Also, you have turned on PID control on motor H and I but you did not set any encoder target. If you don't really mean to do PID control on the motors, you may want to turn PID off. PID might be doing something funny on your motors. Also note that if both button 5 and 6 are pressed together, your code will program the motor with conflicting power in succession. That would be very bad. If I have to guess what you are trying to do, it could be written like this instead.
Code:
        if(joy1Btn(5))
            motor(motorI)=30;
        else if (joy1Btn(6))
            motor[motorI] = -30;
        else
            motor[motorI] = 0;

        if(joy1Btn(2))
            motor(motorH)=-30;
        else if(joy1Btn(4))
            motor(motorH)=30;
        else
            motor[motorH] = 0;

This code does not address the issue of sudden change of power level but at least when the button is released, the motor will stop whereas the previous code won't. And it is less harsh changing 30 to 0 instead of 100 to -100.


Mon Jan 21, 2013 1:42 am
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 2 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.