View unanswered posts | View active topics It is currently Fri Aug 01, 2014 3:53 am






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

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Endcoders better?
That won't work. If you need to do something one after another, you need to use a state machine, like this:
Code:
task main()
{
    int step = 0;

    initializeRobot();
    waitForStart(); // Wait for the beginning of autonomous phase.
    while (true)
    {
        GyroTask(g_Gyro);

        switch (step)
        {
            case 0:
                // step 0: go forward 48 inches.
                SetDriveTarget(48.0);
                step++;
                break;

            case 1:
                // step 1: wait for drive to complete.
                if (g_driveEnabled == false)
                {
                    step++;
                }
                break;

            case 2:
                // step 2: turn right 90 degrees.
                SetTurnTarget(90.0);
                step++;
                break;

            case 3:
                // step 3: wait for turn to complete.
                if (g_turnEnabled == false)
                {
                    step++;
                }
                break;
        }

        TurnTask();
        DriveTask();
        wait1Msec(10);
    }
}


Wed Apr 13, 2011 11:00 pm
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: Endcoders better?
Ok, and also. My fault, but I guess it does help if the left motor is left and the right motor is right. I guess I had them both reversed and both on the wrong sides... Im working on the reprograming now... Thank you so much for your help... there is NO way I would be here without you.


Wed Apr 13, 2011 11:12 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Endcoders better?
Team2844 wrote:
Ok, and also. My fault, but I guess it does help if the left motor is left and the right motor is right. I guess I had them both reversed and both on the wrong sides... Im working on the reprograming now...

That's what I suspected and that's why I asked you to disable the motors, turned the wheels manually and checked the encoder readings. In any case, please label your motors with names that make sense. For example, instead of motorD and motorE, you should say motorLeft and motorRight. That will make it clearer.
Also, make sure you do understand how the code works. If not, ask questions. That's how you learn. If you don't understand how the code works, you won't be able to add features to it.


Wed Apr 13, 2011 11:15 pm
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: Endcoders better?
Ok, with the motors fixed now, and the code that you just gave me. I tested it. But I am still having problems. Right now, the code goes the 48 inches, then starts turning to the right, but on the screen is negative numbers, and the code calls for 90.0 which cant be reached with negatives, so it keeps running around. What am i doing wrong.
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,     motorLeft,     tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     motorRight,    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[motorLeft] = turnPower;
            motor[motorRight] = -turnPower;
        }
        else
        {
            motor[motorLeft] = 0;
            motor[motorRight] = 0;
            g_turnEnabled = false;
        }
        nxtDisplayTextLine(3, "Heading=%f", GyroGetHeading(g_Gyro));
    }
}

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

void SetDriveTarget(float distance)
{
    nMotorEncoder[motorLeft] = 0;
    nMotorEncoder[motorRight] = 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[motorLeft] = BOUND(drivePower + turnPower, -100, 100);
            motor[motorRight] = BOUND(drivePower - turnPower, -100, 100);
        }
        else
        {
            motor[motorLeft] = 0;
            motor[motorRight] = 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()
{
    int step = 0;
   
    initializeRobot();
    waitForStart(); // Wait for the beginning of autonomous phase.
    while (true)
    {
        GyroTask(g_Gyro);

        switch (step)
        {
            case 0:
                // step 0: go forward 48 inches.
                SetDriveTarget(48.0);
                step++;
                break;

            case 1:
                // step 1: wait for drive to complete.
                if (g_driveEnabled == false)
                {
                    step++;
                }
                break;

            case 2:
                // step 2: turn right 90 degrees.
                SetTurnTarget(90.0);
                step++;
                break;

            case 3:
                // step 3: wait for turn to complete.
                if (g_turnEnabled == false)
                {
                    step++;
                }
                break;
        }

        TurnTask();
        DriveTask();
        wait1Msec(10);
    }
}


Wed Apr 13, 2011 11:45 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Endcoders better?
If the gyro is giving you the wrong sign, you probably mounted the gyro up-side-down? You can correct it by either mounting it right-side-up or you can change the following lines in your code:
Code:
In TurnTask:
        float error = GyroGetHeading(g_Gyro) - g_turnTarget;
In DriveTask:
            float turnErr = GyroGetHeading(g_Gyro);


Thu Apr 14, 2011 12:00 am
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: Endcoders better?
Yes, sorry the gyro is mounted upside down. I changed those two lines in the code, but now the robot will only do the 48 inches, and then stop. Any ideas?


Thu Apr 14, 2011 1:12 am
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Endcoders better?
When it stopped after going 48 inches, did you hear the motors still humming? Also look at the screen and see if the "distance" value has reached 48 inches +/- g_driveTolerance? If the motors were humming and the distance value has not quite reach target, can you push the robot and will it start turning? If so, it means you either need to increase g_driveKp or increase g_driveTolerance. PID control couldn't quite get to the target because of steady state error. So by increasing Kp, you give it more power to reach target, you may also increase g_driveTolerance to say 1.0 or 2.0 inches. Let me know if that helps.

Also, please add the following line to task main:
Code:
    while (true)
    {
        GyroTask(g_Gyro);

        nxtDisplayTextLine(5, "step=%d", step); <<< Add this line....
        switch (step)
        {
            case 0:


Last edited by MHTS on Thu Apr 14, 2011 1:26 am, edited 1 time in total.



Thu Apr 14, 2011 1:18 am
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: Endcoders better?
No... it reaches the 48 inches. After that the motors are not humming, and the screen has just gone back to the regular battery voltage screen. It's almost like the program doesnt see that it needs to turn.


Thu Apr 14, 2011 1:25 am
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Endcoders better?
Also add the following lines to your task main, this will stop the battery screen from coming back so you can see the info without worrying about being overwritten. When the robot stopped, tell me what the screen said.
Code:
task main()
{
    int step = 0;
   
   StopTask(displayDiagnostics);
   eraseDisplay();
    initializeRobot();
    waitForStart(); // Wait for the beginning of autonomous phase.
    while (true)
    {
        GyroTask(g_Gyro);

        nxtDisplayTextLine(5, "Step=%d", step);
        switch (step)
        {
            case 0:


Thu Apr 14, 2011 1:31 am
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: Endcoders better?
Heading:0.000000000
Distance:48.25467
Step 4


Thu Apr 14, 2011 1:44 am
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Endcoders better?
Can you post your task main code? Something is not right. It is on step 4, that means it's done.


Thu Apr 14, 2011 1:49 am
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Endcoders better?
Actually, post the whole code. You must have a typo somewhere.


Thu Apr 14, 2011 1:51 am
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: Endcoders better?
Thats what is confusing me.
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,     motorLeft,     tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     motorRight,    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 = GyroGetHeading(g_Gyro) - g_turnTarget;
        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[motorLeft] = turnPower;
            motor[motorRight] = -turnPower;
        }
        else
        {
            motor[motorLeft] = 0;
            motor[motorRight] = 0;
            g_turnEnabled = false;
        }
        nxtDisplayTextLine(3, "Heading=%f", GyroGetHeading(g_Gyro));
    }
}

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

void SetDriveTarget(float distance)
{
    nMotorEncoder[motorLeft] = 0;
    nMotorEncoder[motorRight] = 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[motorLeft] = BOUND(drivePower + turnPower, -100, 100);
            motor[motorRight] = BOUND(drivePower - turnPower, -100, 100);
        }
        else
        {
            motor[motorLeft] = 0;
            motor[motorRight] = 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()
{
    int step = 0;
   
    StopTask(displayDiagnostics);
    eraseDisplay();
    initializeRobot();
    waitForStart(); // Wait for the beginning of autonomous phase.
    while (true)
    {
        GyroTask(g_Gyro);

        nxtDisplayTextLine(5, "Step=%d", step);
        switch (step)
        {
            case 0:
                // step 0: go forward 48 inches.
                SetDriveTarget(48.0);
                step++;
                break;

            case 1:
                // step 1: wait for drive to complete.
                if (g_driveEnabled == false)
                {
                    step++;
                }
                break;

            case 2:
                // step 2: turn right 90 degrees.
                SetTurnTarget(90.0);
                step++;
                break;

            case 3:
                // step 3: wait for turn to complete.
                if (g_turnEnabled == false)
                {
                    step++;
                }
                break;
        }

        TurnTask();
        DriveTask();
        wait1Msec(10);
    }
}


Thu Apr 14, 2011 1:56 am
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Endcoders better?
I can't find anything wrong. Please add the following line to TurnTask and let me know what the screen said.
Code:
        nxtDisplayTextLine(3, "Heading=%f", GyroGetHeading(g_Gyro));
        nxtDisplayTextLine(6, "target=%f,tol=%f", g_turnTarget, g_turnTolerance);


Thu Apr 14, 2011 2:04 am
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: Endcoders better?
Heading:0.000000000
Distance:48.01467
Step 4
Target 90.000000000


Thu Apr 14, 2011 2:14 am
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 133 posts ]  Go to page Previous  1, 2, 3, 4, 5, 6 ... 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.