View unanswered posts | View active topics It is currently Thu Oct 23, 2014 10:29 am






Reply to topic  [ 133 posts ]  Go to page Previous  1, 2, 3, 4, 5 ... 9  Next
Endcoders better? 
Author Message
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: Endcoders better?
Ok, so I think i have everything working in the encoder code, but im a little confused on how I would list my gear ratio. I have it set up that a meduim gear is on the motor, to a small gear, to a large gear. Would that be 80/40/120?


Tue Apr 12, 2011 7:38 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Endcoders better?
The way to look at it is asking the question: how many turns the wheel will make if the motor (encoder) turns exactly one revolution? That's your gear ratio. Assuming you are using the Tetrix gears, not the sprockets (for chain drive), it doesn't matter how many intermediate gears you have. It is one tooth transfer to one tooth. So one revolutoin of the motor will turn 80 teeth. If the wheel is on the 120-tooth gear, then you have 80.0/120.0 (2/3 turn on the wheel).

Wait, where did you mount the encoder? I am assuming the encoder is mounted on the motor, right?

BTW, when you said you have everything working, did you figure out what was the problem with the turn on carpet? Or did you just try the encoder code without using the gyro code?

Also, I have made some minor changes to the code in my previous post so that it can now drive straight using both the encoders and the gyro.


Tue Apr 12, 2011 7:50 pm
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: Endcoders better?
MHTS wrote:
Wait, where did you mount the encoder? I am assuming the encoder is mounted on the motor, right?


Yes

MHTS wrote:
BTW, when you said you have everything working, did you figure out what was the problem with the turn on carpet? Or did you just try the encoder code without using the gyro code?


It was the extra PID control. I turned it off since you said there was PID control in your code, and it allows it to work anywhere... I was testing it in all different places to make sure it worked.

But the gearing is stll confusing. If I am only putting it to 80:120, what about the little gear in the middle... that speeds it up right, causing something different from the 2/3 ratio. I put in the 80:120 ratio, and tested it to go 12 inches, but it kept going.. it wouldnt stop. It would run straight. That was with the code before the update. I just added the new code you updated... and it runs, no stopping, and its running with a turn to the right constantly.

Code:
#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S2,     Magnet,              sensorHiTechnicMagnetic)
#pragma config(Sensor, S3,     gyro,                sensorI2CCustom)
#pragma config(Sensor, S4,     IRSeeker2,           sensorHiTechnicIRSeeker1200)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorNormal, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     motorF,        tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorNormal, PIDControl, encoder)
#pragma config(Servo,  srvo_S1_C3_1,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C3_5,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C3_6,    ,                     tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.
#include "C:\Documents and Settings\FIRST\My Documents\Robotics\Code\gyro.h"
#include "C:\Documents and Settings\FIRST\My Documents\Robotics\Code\Encoders.h"

#define BOUND(n, l, h) (((n) < (l))? (l): ((n) > (h))? (h): (n))

GYRO  g_Gyro;
float g_turnTarget = 0.0;
bool  g_turnEnabled = false;
float g_turnTolerance = 0.5;    //needs to be tuned
float g_turnKp = 0.5;             //needs to be tuned
float g_driveTarget = 0.0;
bool  g_driveEnabled = false;
float g_driveTolerance = 0.5;
float g_driveKp = 1.0;

void SetTurnTarget(float angle)         //This is saying unreferenced function
{
    GyroReset(g_Gyro);
    g_turnTarget = angle;
    g_turnEnabled = true;
}

void TurnTask()
{
    if (g_turnEnabled)
    {
        float error = g_turnTarget - GyroGetHeading(g_Gyro);
        if (error > g_turnTolerance)
        {
            //
            // Simple proportional PID control.
            // Limit the outpout to the range of -50 to 50.
            //
            int turnPower = BOUND((int)(g_turnKp*error), -50, 50);
            motor[motorD] = 50;
            motor[motorE] = -50;
        }
        else
        {
            motor[motorD] = 0;
            motor[motorE] = 0;
            g_turnEnabled = false;
        }
        nxtDisplayTextLine(3, "Heading=%f", GyroGetHeading(g_Gyro));
    }
}

float GetDriveDistance()
{
    return (float)(nMotorEncoder[motorD] + nMotorEncoder[motorE])/2.0*INCHES_PER_CLICK;
}

void SetDriveTarget(float distance)
{
    nMotorEncoder[motorD] = 0;
    nMotorEncoder[motorE] = 0;
    GyroReset(g_Gyro);
    g_driveTarget = distance;
    g_driveEnabled = true;
}

void DriveTask()
{
    if (g_driveEnabled)
    {
        float driveErr = g_driveTarget - GetDriveDistance();

        if (driveErr > g_driveTolerance)
        {
            //
            // Simple proportional PID control.
            // Limit the outpout to the range of -100 to 100 for drive
            // and -50 to 50 for turn
            //
            int drivePower = BOUND((int)(g_driveKp*driveErr), -100, 100);
            float turnErr = -GyroGetHeading(g_Gyro);
            int turnPower = BOUND((int)(g_turnKp*turnErr), -50, 50);
            motor[motorD] = BOUND(80 + 50, -100, 100);
            motor[motorE] = BOUND(80 - 50, -100, 100);
        }
        else
        {
            motor[motorD] = 0;
            motor[motorE] = 0;
            g_driveEnabled = false;
        }
        nxtDisplayTextLine(4, "Distance=%f", GetDriveDistance());
   }
}

void initializeRobot()
{
  servoTarget[servo1] = 247;
  servoTarget[servo2] = 0;
  servoTarget[servo3] = 100;
  servoTarget[servo4] = 205;
  servoTarget[servo5] = 49;
  servoTarget[servo6] = 136;
  // 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.
  GyroInit(g_Gyro, gyro, 0);

  return;
}

task main()
{
    initializeRobot();
    waitForStart(); // Wait for the beginning of autonomous phase.
    SetDriveTarget(12.0);  //go forward 12 inches
    while (true)
    {
        GyroTask(g_Gyro);
        TurnTask();
        DriveTask();
        wait1Msec(10);
    }
}


Tue Apr 12, 2011 11:56 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Endcoders better?
Team2844 wrote:
But the gearing is stll confusing. If I am only putting it to 80:120, what about the little gear in the middle... that speeds it up right, causing something different from the 2/3 ratio. I put in the 80:120 ratio, and tested it to go 12 inches, but it kept going.. it wouldnt stop. It would run straight. That was with the code before the update. I just added the new code you updated... and it runs, no stopping, and its running with a turn to the right constantly.

I am assuming the 80-tooth gear is on the motor then it interfaces with the 40-tooth gear. The 40-tooth gear interfaces with the 120-tooth gear and is on the wheel, right? If you turn the motor one revolution, 80 teeth on the motor gear moved. This in turn moved the 40-tooth gear 2 turns. So the gear ratio between the 80 and the 40-tooth is 80:40 (2:1), right? Now for the 40:120, it is (1:3). Then (2/1)*(1/3) = 2/3. But then it is exactly identical to 80:120 (2/3). So it doesn't matter how many gears you used in the middle and what they are, only the ones on the motor and the wheel counts. In fact, why do you have the 40-tooth gear anyway? If you want to visualize this, get a marker and make a mark on the motor gear (80) and the wheel gear (120). Turn the motor gear one turn and note how much the 120-tooth gear has turned.
I hope that makes it clear for you.
Regarding the weird behavior you are seeing. First, you have changed my code. In TurnTask and DriveTask:
Code:
void TurnTask()
{
    if (g_turnEnabled)
    {
        float error = g_turnTarget - GyroGetHeading(g_Gyro);
        if (error > g_turnTolerance)
        {
            //
            // Simple proportional PID control.
            // Limit the outpout to the range of -50 to 50.
            //
            int turnPower = BOUND((int)(g_turnKp*error), -50, 50);
            motor[motorD] = 50;  //this should be turnPower instead of 50
            motor[motorE] = -50;  //this should be -turnPower instead of -50
        }
        else
        {
            motor[motorD] = 0;
            motor[motorE] = 0;
            g_turnEnabled = false;
        }
        nxtDisplayTextLine(3, "Heading=%f", GyroGetHeading(g_Gyro));
    }
}

void DriveTask()
{
    if (g_driveEnabled)
    {
        float driveErr = g_driveTarget - GetDriveDistance();

        if (driveErr > g_driveTolerance)
        {
            //
            // Simple proportional PID control.
            // Limit the outpout to the range of -100 to 100 for drive
            // and -50 to 50 for turn
            //
            int drivePower = BOUND((int)(g_driveKp*driveErr), -100, 100);
            float turnErr = -GyroGetHeading(g_Gyro);
            int turnPower = BOUND((int)(g_turnKp*turnErr), -50, 50);
            motor[motorD] = BOUND(80 + 50, -100, 100); //This should be drivePower + turnPower instead of 80 + 50
            motor[motorE] = BOUND(80 - 50, -100, 100);  //This should be drivePower - turnPower instead of 80 - 50
        }
        else
        {
            motor[motorD] = 0;
            motor[motorE] = 0;
            g_driveEnabled = false;
        }
        nxtDisplayTextLine(4, "Distance=%f", GetDriveDistance());
   }
}

When driving straight, turnPower should be almost 0. But since you replaced them with + 50 and - 50, you are telling the robot to turn even though the gyro said you are straight.


Wed Apr 13, 2011 12:56 am
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: Endcoders better?
Ya, I see it now. Thanks. The way our robot is built needs the 40 tooth gear. Any idea on why the encoder code isnt working? I can't figure it out.

Edit. O OK. I will try that.


Wed Apr 13, 2011 1:11 am
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Endcoders better?
I have edited my previous post to explain why.


Wed Apr 13, 2011 1:13 am
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: Endcoders better?
Ok, so I can see the distance that it travels on the screen, but when I am trying to run it, the motors wont move.. i have to push it for the distance to go up.

The distance is in negatives though when it is running forward.. is it suppose to be like that? I want it to run just 12 inches for the testing, but it keeps moving after i push it to get it going.


Wed Apr 13, 2011 1:20 am
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Endcoders better?
Team2844 wrote:
Ok, so I can see the distance that it travels on the screen, but when I am trying to run it, the motors wont move.. i have to push it for the distance to go up.

The distance is in negatives though when it is running forward.. is it suppose to be like that? I want it to run just 12 inches for the testing, but it keeps moving after i push it to get it going.

12 inches is quite small. If your Kp is still 1.0, it will give you a drivePower of 12*1.0 = 12. That's hardly anything comparing to a full scale of 100. It's time to tune Kp. Set it to something big like 50.0 and try it. Like I said, you need to tune Kp for PID to work right.
Regarding the negative number on the screen, are you sure you are pushing the robot the right side forward? If you push the robot backwards, the number will go negative. If you are sure the robot is facing the correct direction, then you may have the motors polarity backward. If so, you can change your pragmas to inverse both left and right motors.


Wed Apr 13, 2011 1:55 am
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: Endcoders better?
I push the robot "forward" but I get negatives. What do I need to change in the pragrams then. If I just switch both of them in the reverse check box (one not checked), it goes backwards with negatives... this is all with the same code(the code is going forward right?). Is there something I need to change in the pragrams that I dont see. Im sorry for being such a rookie/noob at this... haha.


Wed Apr 13, 2011 2:22 am
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Endcoders better?
That's weird. It means the encoders are not in sync with the motor direction. I don't know how this could happen. Did you make your own encoder cables that are backward? In any case, if you can't figure out what's wrong, you can correct it in the code, just put a negative in the GetDriveDistance function:
Code:
float GetDriveDistance()
{
    return (float)(nMotorEncoder[motorD] + nMotorEncoder[motorE])/-2.0*INCHES_PER_CLICK;
}


Wed Apr 13, 2011 2:34 am
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Endcoders better?
Ok, let's make it clearer. Is motor D the left motor and motor E the right motor? You may want to rename your motors to motorLeft and motorRight so that it has no ambiguity on which side is which. That will affect the direction it turns etc. Also, please post your updated pragmas.

Wait, I looked at your code closer. What is encoders.h? Can you post that?


Wed Apr 13, 2011 2:44 am
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: Endcoders better?
I am using the encoder cables thats were in the kit. Motor D is Left and MotorE is right. My Pragrams are the same as posted above again. I changed that one line of code that you said, and it does go the distance... but when it gets to the point. The Left motor stops and the right goes just a little more, turning the robot. Is that juust a tuneing that I need to do? Encoders.h is just this that you gave me.
Code:
#define PI                      3.14159
#define CLICKS_PER_REVOLUTION   1440.0          //encoder clicks per revolution
#define GEAR_RATIO              (24.0/16.0)     //motor:wheel=24:16 (use your own gear ratio)
#define WHEEL_DIAMETER          4.0  //use your own wheel size
#define WHEEL_CIRCUMFERENCE     (PI*WHEEL_DIAMETER)
#define INCHES_PER_CLICK        (WHEEL_CIRCUMFERENCE*GEAR_RATIO/CLICKS_PER_REVOLUTION)


Wed Apr 13, 2011 9:54 am
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Endcoders better?
That's very strange. Then you need to debug it further. First, comment out the code that runs the motors (i.e. all the lines with motor[motorD] and motor[motorE]) in TurnTask and DriveTask so the motors don't run. Then change the GetDriveDistance function to:
Code:
float GetDriveDistance()
{
    nxtDisplayTextLine(5, "L=%d,R=%d", nMotorEncoder[motorD], nMotorEncoder[motorE]);
    return (float)(nMotorEncoder[motorD] + nMotorEncoder[motorE])/2.0*INCHES_PER_CLICK;
}

Run the program and manually turn the left wheel forward and backward and note the L value on screen to see if the numbers are increasing or decreasing as expected. It should increase when the left wheel is moving forward, decrease when moving backward. Do the same to the right wheel. If when you are moving the left wheel, the R value changed instead. Then you have swapped left and right (i.e. motorE is actually left and motorD is right). If the left and right wheels are correct but the direction is wrong (i.e. moving the left wheel forward yielded decreasing L value) then flip the reverse check box for motor D in the pragmas. Do the same for the right wheel. Make sure the reverse check-box for motor D and E are different (one check and the other not). If that's all fixed, uncomment the motor lines and run the program. If the robot is still going backward (which is strange) then go ahead and change the 2.0 in the GetDriveDistance function to -2.0 and flip the check boxes for both motors.

Let me know what you find.
Team2844 wrote:
I changed that one line of code that you said, and it does go the distance... but when it gets to the point. The Left motor stops and the right goes just a little more, turning the robot. Is that juust a tuneing that I need to do? Encoders.h is just this that you gave me.

Yep, that is probably because Kp was too big so the robot was going too fast (not slowing down enough when approaching target) so it abruptly stopped causing a jerk action which turned the robot but then the gyro detected the change and tried to correct it. It probably couldn't because g_turnKp was too small.

So, we need to tune all the parameters. First, please correct the gear ratio in encoders.h. It still contains (24.0/16.0) which is wrong for your robot. It should be (80.0/120.0). And also please make sure the parameters are really correct for your robot (e.g. wheel diameter etc). You don't want to tune the parameters with the wrong data.

Then, we want to tune g_turnKp. Change the main task to turn right 360-degree like this:
Code:
task main()
{
    initializeRobot();
    waitForStart(); // Wait for the beginning of autonomous phase.
    SetTurnTarget(360.0);  //turn right 360 degrees
    while (true)
    {
        GyroTask(g_Gyro);
        TurnTask();
        DriveTask();
        wait1Msec(10);
    }
}

Set g_turnKp to something big (e.g. 50.0). Run the program. The robot will probably overshoot (turning more than 360) and may oscillate left and right before stopping. Start decreasing g_turnKp until the turn stops smoothly without jerking. I'd bet it is between 10 and 30. This depends on the weight of your robot and the strength of the motor and gear ratio etc. That's why it is important to tune it to your robot, not use my values.

Then, we want to tune g_driveKp next. First comment out the turnErr code in DriveTask like this so that we will concentrate tuning the encoders only and not having the gyro interfering:
Code:
            int drivePower = BOUND((int)(g_driveKp*driveErr), -100, 100);
            float turnErr = 0; //-GyroGetHeading(g_Gyro);
            int turnPower = BOUND((int)(g_turnKp*turnErr), -50, 50);
            motor[motorD] = BOUND(drivePower + turnPower, -100, 100);
            motor[motorE] = BOUND(drivePower - turnPower, -100, 100);

Change the distance from 12.0 inches to something bigger. If you have the space, make it 8 ft (96.0) If not, make it at least 4 ft (48.0). The longer the distance, the more accurate you can tune the parameters (allowing the robot to fully accelerate and decelerate).
Code:
task main()
{
    initializeRobot();
    waitForStart(); // Wait for the beginning of autonomous phase.
    SetDriveTarget(96.0);  //go forward 8 ft
    while (true)
    {
        GyroTask(g_Gyro);
        TurnTask();
        DriveTask();
        wait1Msec(10);
    }
}

Set g_driveKp to something big (e.g. 50.0 or 100.0) and run the program. The robot will overshoot the target and may even drive back and forth (oscillates) to correct the distance. Start decreasing g_driveKp until the robot stops smoothly without jerking.

After tunring the g_driveKp, let's undo the changes we made to turnErr in DriveTask back to:
Code:
            float turnErr = -GyroGetHeading(g_Gyro);

Next, let's tune the correct driving distance. Although I gave you the equations to calculate INCHES_PER_CLICK, that's only an approximate value. You need to fine tune this. Pull out a calculator and calculate the value according to those equations I gave you. Make sure you use the correct gear ratio. For example, with the following parameters for your robot, INCHES_PER_CLICK comes out 0.005817764.
Code:
#define PI                      3.14159
#define CLICKS_PER_REVOLUTION   1440.0          //encoder clicks per revolution
#define GEAR_RATIO              (80.0/120.0)    //motor:wheel=80:120
#define WHEEL_DIAMETER          4.0             //use your own wheel size
#define WHEEL_CIRCUMFERENCE     (PI*WHEEL_DIAMETER)
#define INCHES_PER_CLICK        (WHEEL_CIRCUMFERENCE*GEAR_RATIO/CLICKS_PER_REVOLUTION)

Change the include encoders.h line in your code to define the actual value like this:
Code:
//#include "C:\Documents and Settings\FIRST\My Documents\Robotics\Code\Encoders.h"
#define INCHES_PER_CLICK        0.005817764

Use masking tape and make a start line on your floor. Measure 8 ft (or whatever distance you set your drive target) and make a finish line on the floor. Run the program and see how close you are getting to the target. If you stop short, decrease the value for INCHES_PER_CLICK. If you are over, increase the value.

After tuning all these parameters, you should now have a robot that turns and drives very accurately in autonomous.


Wed Apr 13, 2011 12:00 pm
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: Endcoders better?
Ok, Thank you.

I have been working on the robot with what you have said. I am having trouble with the turning, but I am not on the playing field. Tommorow I will have access to the field so I will try it there. But I have gotten the encoders to work. The code. Questions though.

Code:
#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S2,     Magnet,              sensorHiTechnicMagnetic)
#pragma config(Sensor, S3,     gyro,                sensorI2CCustom)
#pragma config(Sensor, S4,     IRSeeker2,           sensorHiTechnicIRSeeker1200)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorNormal, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     motorF,        tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorNormal, PIDControl, encoder)
#pragma config(Servo,  srvo_S1_C3_1,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C3_5,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C3_6,    ,                     tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.
#include "C:\Documents and Settings\Mom\My Documents\Robotics\Code\gyro.h"
#include "C:\Documents and Settings\Mom\My Documents\Robotics\Code\Encoders7.h"

#define BOUND(n, l, h) (((n) < (l))? (l): ((n) > (h))? (h): (n))

GYRO  g_Gyro;
float g_turnTarget = 0.0;
bool  g_turnEnabled = false;
float g_turnTolerance = 0.5;    //needs to be tuned
float g_turnKp = 1.0;             //needs to be tuned
float g_driveTarget = 0.0;
bool  g_driveEnabled = false;
float g_driveTolerance = 0.5;
float g_driveKp = 35.0;

void SetTurnTarget(float angle)
{
    GyroReset(g_Gyro);
    g_turnTarget = angle;
    g_turnEnabled = true;
}

void TurnTask()
{
    if (g_turnEnabled)
    {
        float error = g_turnTarget - GyroGetHeading(g_Gyro);
        if (error > g_turnTolerance)
        {
            //
            // Simple proportional PID control.
            // Limit the outpout to the range of -70 to 70.
            //
            int turnPower = BOUND((int)(g_turnKp*error), -70, 70);
            motor[motorD] = turnPower;
            motor[motorE] = -turnPower;
        }
        else
        {
            motor[motorD] = 0;
            motor[motorE] = 0;
            g_turnEnabled = false;
        }
        nxtDisplayTextLine(3, "Heading=%f", GyroGetHeading(g_Gyro));
    }
}

float GetDriveDistance()
{
    return (float)(nMotorEncoder[motorD] + nMotorEncoder[motorE])/-2.0*INCHES_PER_CLICK;
}

void SetDriveTarget(float distance)
{
    nMotorEncoder[motorD] = 0;
    nMotorEncoder[motorE] = 0;
    GyroReset(g_Gyro);
    g_driveTarget = distance;
    g_driveEnabled = true;
}

void DriveTask()
{
    if (g_driveEnabled)
    {
        float driveErr = g_driveTarget - GetDriveDistance();

        if (driveErr > g_driveTolerance)
        {
            //
            // Simple proportional PID control.
            // Limit the outpout to the range of -100 to 100 for drive
            // and -50 to 50 for turn
            //
            int drivePower = BOUND((int)(g_driveKp*driveErr), -100, 100);
            float turnErr = -GyroGetHeading(g_Gyro);
            int turnPower = BOUND((int)(g_turnKp*turnErr), -70, 70);
            motor[motorD] = BOUND(drivePower + turnPower, -100, 100);
            motor[motorE] = BOUND(drivePower - turnPower, -100, 100);
        }
        else
        {
            motor[motorD] = 0;
            motor[motorE] = 0;
            g_driveEnabled = false;
        }
        nxtDisplayTextLine(4, "Distance=%f", GetDriveDistance());
   }
}

void initializeRobot()
{
  servoTarget[servo1] = 247;
  servoTarget[servo2] = 0;
  servoTarget[servo3] = 100;
  servoTarget[servo4] = 205;
  servoTarget[servo5] = 49;
  servoTarget[servo6] = 136;
  // 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.
  GyroInit(g_Gyro, gyro, 0);

  return;
}

task main()
{
    initializeRobot();
    waitForStart(); // Wait for the beginning of autonomous phase.
    SetDriveTarget(48.0);  //go forward 48 inches
    SetTurnTarget(90.0); // Turn 90
    while (true)
    {
        GyroTask(g_Gyro);
        TurnTask();
        DriveTask();
        wait1Msec(10);
    }
   
}


Right now. The code will run the robot the 48 inches (all the room i have right now) and will stop. It runs straight, but thats with the float g_turnKp where it is. The Pragrams.
Code:
#define PI                      3.14159
#define CLICKS_PER_REVOLUTION   1440.0          //encoder clicks per revolution
#define GEAR_RATIO              (24.0/16.0)     //motor:wheel=24:16 (use your own gear ratio)
#define WHEEL_DIAMETER          4.0  //use your own wheel size
#define WHEEL_CIRCUMFERENCE     (PI*WHEEL_DIAMETER)
#define INCHES_PER_CLICK        (0.005217764)

I had to change the Inches per click for it to reach the targeted area.

A question though. You may see above that I am trying to get the robot to turn after the 48 inches. Right now it just runs the 48 inches, and stops... is there something I am doing wrong?


Wed Apr 13, 2011 10:31 pm
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: Endcoders better?
I just change the code a little, changed it above in the post.

I go the 48 inches, and then it starts to turn, but the robot turns the wrong way. If I try to put in -90.0 it wont work, how am I to turn the robot the other way.


Wed Apr 13, 2011 10:43 pm
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 133 posts ]  Go to page Previous  1, 2, 3, 4, 5 ... 9  Next

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.