View unanswered posts | View active topics It is currently Fri Apr 18, 2014 11:44 am






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

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: Endcoders better?
That didnt do anything. When I start the autonomous it is starting at the 45 already. That is the same number of what i want to go to. Its not zeroing. I have the encoder plugged into the right spot, i already tried it in the other encoder slot, and the motor will move with the number on the screen staying at 45, so i know i have it in the right spot. When I run it... it gets a little more than 90 degrees before i shut it off from running into the bottom of the robot, with the numbers increasing from the 45 it started with. Here is the full code.
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,     lights,        tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     motorArm,      tmotorNormal, openLoop, reversed, encoder)
#pragma config(Servo,  srvo_S1_C3_1,    trailer1,             tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    trailer2,             tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    gate,                 tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C3_5,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C3_6,    twist,                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\Encoders16.h"

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

GYRO  g_Gyro;
float g_armTarget = 0.0;
bool  g_armEnabled = false;
float g_armTolerance = 0.5;
float g_armKp = 10.0;

void SetArmTarget(float angle)
{
  g_armTarget = angle;
  g_armEnabled = true;
}

float GetArmPosition()
{
    return (float)nMotorEncoder[motorArm]*DEGREES_PER_CLICK;
}

void ArmTask()
{
  if (g_armEnabled)
  {
    float error = g_armTarget - GetArmPosition();
    nxtDisplayTextLine(4, "ArmErr=%f", error);
    if (abs(error) > g_armTolerance)
    {
      motor[motorArm] = BOUND((int)(g_armKp*error), -50, 50);
    }
    else
    {
      motor[motorArm] = 0;
      g_armEnabled = false;
    }
  }
}

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();
    nMotorEncoder[motorArm] = 0;
    initializeRobot();
    waitForStart(); // Wait for the beginning of autonomous phase.
    while (true)
    {
        GyroTask(g_Gyro);

        nxtDisplayTextLine(5, "Step=%d", step);
        switch (step)
        {
//*****************************Drive to trailer*************************************
            case 0:
                // step 0: go forward.
                SetArmTarget(45);
                ClearTimer(T1);
                step++;
                break;

            case 1:
                // step 3: wait to complete.
                if ((time1[T1] > 500)&& (g_armEnabled == false))
                {
                    step++;
                }
                break;

        }

        ArmTask();
        wait1Msec(10);
    }
}


and here is the degrees per click... did I add it in wrong. I have the arm set up as a (motor)40:120:40:120(arm attached) gearing, which makes it 40:120 right?
Code:
#define PI                      3.14159
#define CLICKS_PER_REVOLUTION   1440.0          //encoder clicks per revolution
#define GEAR_RATIO              (40.0/120.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.005807764)
#define DEGREES_PER_CLICK       (0.00290887962672075)


I put it in with the drive encoders... is that a problem.


Fri Apr 22, 2011 1:47 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1343
Post Re: Endcoders better?
Team2844 wrote:
and here is the degrees per click... did I add it in wrong. I have the arm set up as a (motor)40:120:40:120(arm attached) gearing, which makes it 40:120 right?
Code:
#define PI                      3.14159
#define CLICKS_PER_REVOLUTION   1440.0          //encoder clicks per revolution
#define GEAR_RATIO              (40.0/120.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.005807764)
#define DEGREES_PER_CLICK       (0.00290887962672075)


I put it in with the drive encoders... is that a problem.

It is not a problem that you put these in the encoders.h file but you should just put all these lines into your main program so it is easier to see than have to go to another file. But in any case, you should not use the same equations as the drive train to calculate the value. The equations are for translating the encoder clicks to distance travel in inches on 4-inch wheels. Your arm does not travel in inches (at least not on 4-inch wheels). With the gear ratio you mentioned, it means when the motor turns one revolution, the arm will turn 40:120 = 1/3. One revolution of the encoder has 1440 clicks so 1/3 turn of the arm is equivalent to 1440 clicks. 1/3 turn is 120 degrees so DEGREES_PER_CLICK should have been 120/1440 = 0.08333. Also, the encoder reset line is in the wrong place. It should be after waitForStart, not before it.


Fri Apr 22, 2011 2:07 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1343
Post Re: Endcoders better?
If you have a chance, you should post some close up pictures of your robot. They will help me see what the arrangements are (drive train, arm, trailers etc). Right now, I have to imagine your arrangement.


Fri Apr 22, 2011 2:15 pm
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: Endcoders better?
With the .083333 in there, numbers spike really high at 400 range when i shut it off.. which 480 clicks would be 90 degrees.
With .033333 in there, the numbers get to 135 which is 90 degrees from where it started.

Are those numbers on the nxt screen going off of degrees, or clicks?

It is still starting from 45 on both of these tries, not 0. Is there a line of code anywhere that i'm missing, because it wont zero out before it runs.

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,     lights,        tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     motorArm,      tmotorNormal, openLoop, reversed, encoder)
#pragma config(Servo,  srvo_S1_C3_1,    trailer1,             tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    trailer2,             tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    gate,                 tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C3_5,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C3_6,    twist,                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"

#define BOUND(n, l, h) (((n) < (l))? (l): ((n) > (h))? (h): (n))
#define PI                      3.14159
#define CLICKS_PER_REVOLUTION   1440.0          //encoder clicks per revolution
#define GEAR_RATIO              (40.0/120.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.005807764)
#define DEGREES_PER_CLICK       (0.0333)

GYRO  g_Gyro;
float g_armTarget = 0.0;
bool  g_armEnabled = false;
float g_armTolerance = 0.5;
float g_armKp = 10.0;

void SetArmTarget(float angle)
{
  g_armTarget = angle;
  g_armEnabled = true;
}

float GetArmPosition()
{
    return (float)nMotorEncoder[motorArm]*DEGREES_PER_CLICK;
}

void ArmTask()
{
  if (g_armEnabled)
  {
    float error = g_armTarget - GetArmPosition();
    nxtDisplayTextLine(4, "ArmErr=%f", error);
    if (abs(error) > g_armTolerance)
    {
      motor[motorArm] = BOUND((int)(g_armKp*error), -50, 50);
    }
    else
    {
      motor[motorArm] = 0;
      g_armEnabled = false;
    }
  }
}

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.
    nMotorEncoder[motorArm] = 0;
    while (true)
    {
        GyroTask(g_Gyro);

        nxtDisplayTextLine(5, "Step=%d", step);
        switch (step)
        {
//*****************************Drive to trailer*************************************
            case 0:
                // step 0: go forward.
                SetArmTarget(45);
                ClearTimer(T1);
                step++;
                break;

            case 1:
                // step 3: wait to complete.
                if ((time1[T1] > 500)&& (g_armEnabled == false))
                {
                    step++;
                }
                break;

        }

        ArmTask();
        wait1Msec(10);
    }
}


Fri Apr 22, 2011 2:40 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1343
Post Re: Endcoders better?
Oh I see what problem you have. The nxt display is showing you the ArmErr not the arm position. It is meant to show you why it is not stopping. It should stop when ArmErr is close to zero (within the tolerance). If you want to show the arm postion, you should do this:
Code:
void ArmTask()
{
  if (g_armEnabled)
  {
    float error = g_armTarget - GetArmPosition();
    nxtDisplayTextLine(4, "ArmPos=%f", GetArmPosition());
    if (abs(error) > g_armTolerance)
    {
      motor[motorArm] = BOUND((int)(g_armKp*error), -50, 50);
    }
    else
    {
      motor[motorArm] = 0;
      g_armEnabled = false;
    }
  }
}


Fri Apr 22, 2011 2:50 pm
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: Endcoders better?
I have only changed that line of code... and it is saying that the arm is going into the negatives. If i reverse the motor, I am still going into the negatives when it runs. How to I fix that?


Fri Apr 22, 2011 3:23 pm
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: Endcoders better?
NVM.. i guess it helps if i wire the motor right.... haha, its the little things that get in the way. It is working now.


Fri Apr 22, 2011 3:25 pm
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: Endcoders better?
Ok, one question after another. I have the Ir sensor on the robot, and i have the code to run the sensor via my old time code. I want to add it into the new code, and want it to run for an amount of time, then switch to the next case. How would i add that into the code. I have something like this... below... but it will keep running past the amount of time.
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,     lights,        tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     motorArm,      tmotorNormal, openLoop, encoder)
#pragma config(Servo,  srvo_S1_C3_1,    trailer1,             tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    trailer2,             tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    gate,                 tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C3_5,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C3_6,    twist,                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\Encoders12.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.3;    //needs to be tuned
float g_turnKp = 24.0;             //needs to be tuned
float g_driveTarget = 0.0;
bool  g_driveEnabled = false;
float g_driveTolerance = 0.5;
float g_driveKp = 50.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 (abs(error) > g_turnTolerance)
        {
            //
            // Simple proportional PID control.
            // Limit the outpout to the range of -70 to 70.
            //
            int turnPower = BOUND((int)(g_turnKp*error), -100, 100);
            motor[motorLeft] = turnPower;
            motor[motorRight] = -turnPower;
        }
        else
        {
            motor[motorLeft] = 0;
            motor[motorRight] = 0;
            g_turnEnabled = false;
            nxtDisplayTextLine(7, "err=%f", error);
        }
        nxtDisplayTextLine(3, "Heading=%f", GyroGetHeading(g_Gyro));
        nxtDisplayTextLine(6, "target=%f,tol=%f", g_turnTarget, g_turnTolerance);
    }
}

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 (abs(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), -100, 100);
            motor[motorLeft] = BOUND(drivePower + turnPower, -100, 100);
            motor[motorRight] = BOUND(drivePower - turnPower, -100, 100);
            nxtDisplayTextLine(7, "drvErr=%f", driveErr);
        }
        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)
        {
//*****************************Drive to opp side of field*************************************

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

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

            case 2:
                // step 2: ir seeker.
                 ClearTimer(T1);   
                 while(true)
                  {
                    if(SensorValue[IRSeeker2] == 5)
                    {
                      motor[motorLeft] = 50;
                      motor[motorRight] = 50;
                    }

                     if(SensorValue[IRSeeker2] > 5)
                    {
                      motor[motorLeft] = 70;
                      motor[motorRight] = -70;
                    }

                     if(SensorValue[IRSeeker2] < 5)
                    {
                      motor[motorLeft] = -70;
                      motor[motorRight] = 70;
                    }
                  }
               
                step++;
                break;

            case 3:
                // step 3: wait for drive to complete.
                if (time1[T1] > 6000)
                {
                    motor[motorLeft] = 0;
                    motor[motorRight] = 0;
                    step++;
                }
                break;

        }

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


I know that im not doing something wrong, but dont know where to go. Could you help me with this?


Fri Apr 22, 2011 3:35 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1343
Post Re: Endcoders better?
No, don't put any loop in a state machine. That defeats the purpose of using a state machine that is to avoid blocking (while-loop blocks the state machine from doing anything else). Instead, do it similar to the DriveTask code except that instead of using the gyro as the heading sensor, use the IR seeker as your heading sensor. So add the following code and change your task main as the following. Note that I use zone 2 of the IR seeker as the straight ahead zone. Please refer to this thread for the reason:
http://www.robotc.net/forums/viewtopic.php?f=52&t=2780
That means you have to physically mount the IR seeker sideway pointing to the right. If you insist to use the wider (more inaccurate zone 5), you can change the "2.0" number in the DriveIRTask back to 5.0, but I recommend doing it sideway. BTW, the code below uses Xander's IRSeeker driver. So you must include HTIRS2-driver.h from Xander's driver suite.
Code:
bool  g_driveIREnabled = false;
float g_turnIRKp = 1.0;    //need to tune this

float IRGetACDir(tSensors link)
{
    static float prevDir = 0.0;
    float currDir;
    int acS[5];
    int idx;

    idx = HTIRS2readACDir(link);
    currDir = (float)idx;
    if (idx == 0)
    {
        currDir = prevDir;
    }
    else if (HTIRS2readAllACStrength(link, acS[0], acS[1], acS[2], acS[3], acS[4]))
    {
        idx = (idx - 1)/2;
        if ((idx < 4) && (acS[idx] != 0) && (acS[idx + 1] != 0))
        {
            currDir += (float)(acS[idx + 1] - acS[idx])/
                          max(acS[idx], acS[idx + 1]);
        }
    }
    prevDir = currDir;

    return currDir;
}

void SetDriveIRTarget(float distance)
{
    nMotorEncoder[motorLeft] = 0;
    nMotorEncoder[motorRight] = 0;
    g_driveTarget = distance;
    g_driveIREnabled = true;
}

void DriveIRTask()
{
    if (g_driveIREnabled)
    {
        float driveErr = g_driveTarget - GetDriveDistance();

        if (abs(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 = IRGetACDir(IRSeeker2) - 2.0;  //use zone 2 as the straight ahead zone
            int turnPower = BOUND((int)(g_turnIRKp*turnErr), -50, 50);
            motor[motorLeft] = BOUND(drivePower + turnPower, -100, 100);
            motor[motorRight] = BOUND(drivePower - turnPower, -100, 100);
        }
        else
        {
            motor[motorLeft] = 0;
            motor[motorRight] = 0;
            g_driveIREnabled = false;
        }
   }
}

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)
        {
//*****************************Drive to opp side of field*************************************

           case 0:
                // step 0: go forward 12 inches.
                SetDriveTarget(12.0);
                step++;
                break;

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

            case 2:
                // step 2: ir seeker drive.
                SetDriveIRTarget(<whatever distance>);
                step++;
                break;

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

        }

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



Last edited by MHTS on Fri Apr 22, 2011 5:20 pm, edited 2 times in total.



Fri Apr 22, 2011 4:50 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1343
Post Re: Endcoders better?
Note that when going over the mountain, the IR seeker may lose sight of the IR beacon, so it is better to break up the drive into two segments. The first segment should use the normal SetDriveTarget to get over the mountain. After you are on the other side, then use SetDriveIRTarget to go the rest of the way. Also note that going over the mountain may screw up the "horizontal distance" of the encoder. So you may need to experiment the exact distance to reach the dispenser. In order to be more accurate, we put a touch sensor in the front so when we run into the dispenser, we will know we are there.


Fri Apr 22, 2011 5:03 pm
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: Endcoders better?
The code.
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,     lights,        tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     motorArm,      tmotorNormal, openLoop, encoder)
#pragma config(Servo,  srvo_S1_C3_1,    trailer1,             tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    trailer2,             tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    gate,                 tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C3_5,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C3_6,    twist,                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:\Projects\rdpartyrobotcdr-v1.8.1-HT\drivers\HTIRS2-driver.h"

#define BOUND(n, l, h) (((n) < (l))? (l): ((n) > (h))? (h): (n))
#define PI                      3.14159
#define CLICKS_PER_REVOLUTION   1440.0          //encoder clicks per revolution
#define GEAR_RATIO              (40.0/120.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.005807764)
#define DEGREES_PER_CLICK       (0.03333)

GYRO  g_Gyro;
float g_turnTarget = 0.0;
bool  g_turnEnabled = false;
float g_turnTolerance = 0.3;    //needs to be tuned
float g_turnKp = 24.0;             //needs to be tuned
float g_driveTarget = 0.0;
bool  g_driveEnabled = false;
float g_driveTolerance = 0.5;
float g_driveKp = 50.0;
bool  g_driveIREnabled = false;
float g_turnIRKp = 10.0;    //need to tune this

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 (abs(error) > g_turnTolerance)
        {
            //
            // Simple proportional PID control.
            // Limit the outpout to the range of -70 to 70.
            //
            int turnPower = BOUND((int)(g_turnKp*error), -100, 100);
            motor[motorLeft] = turnPower;
            motor[motorRight] = -turnPower;
        }
        else
        {
            motor[motorLeft] = 0;
            motor[motorRight] = 0;
            g_turnEnabled = false;
            nxtDisplayTextLine(7, "err=%f", error);
        }
        nxtDisplayTextLine(3, "Heading=%f", GyroGetHeading(g_Gyro));
        nxtDisplayTextLine(6, "target=%f,tol=%f", g_turnTarget, g_turnTolerance);
    }
}

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 (abs(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), -100, 100);
            motor[motorLeft] = BOUND(drivePower + turnPower, -100, 100);
            motor[motorRight] = BOUND(drivePower - turnPower, -100, 100);
            nxtDisplayTextLine(7, "drvErr=%f", driveErr);
        }
        else
        {
            motor[motorLeft] = 0;
            motor[motorRight] = 0;
            g_driveEnabled = false;
        }
        nxtDisplayTextLine(4, "Distance=%f", GetDriveDistance());
   }
}

float IRGetACDir(tSensors link)
{
    static float prevDir = 0.0;
    float currDir;
    int acS[5];
    int idx;

    idx = HTIRS2readACDir(link);
    currDir = (float)idx;
    if (idx == 0)
    {
        currDir = prevDir;
    }
    else if (HTIRS2readAllACStrength(link, acS[0], acS[1], acS[2], acS[3], acS[4]))
    {
        idx = (idx - 1)/2;
        if ((idx < 4) && (acS[idx] != 0) && (acS[idx + 1] != 0))
        {
            currDir += (float)(acS[idx + 1] - acS[idx])/
                          max(acS[idx], acS[idx + 1]);
        }
    }
    prevDir = currDir;

    return currDir;
}

void SetDriveIRTarget(float distance)
{
    nMotorEncoder[motorLeft] = 0;
    nMotorEncoder[motorRight] = 0;
    g_driveTarget = distance;
    g_driveIREnabled = true;
}

void DriveIRTask()
{
    if (g_driveIREnabled)
    {
        float driveErr = g_driveTarget - GetDriveDistance();

        if (abs(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 = IRGetACDir(IRSeeker2) - 2.0;  //use zone 2 as the straight ahead zone
            int turnPower = BOUND((int)(g_turnIRKp*turnErr), -50, 50);
            motor[motorLeft] = BOUND(drivePower + turnPower, -100, 100);
            motor[motorRight] = BOUND(drivePower - turnPower, -100, 100);
        }
        else
        {
            motor[motorLeft] = 0;
            motor[motorRight] = 0;
            g_driveIREnabled = false;
        }
   }
}

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)
        {
//*****************************Drive to opp side of field*************************************

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

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

            case 2:
                // step 2: ir seeker drive.
                SetDriveIRTarget(12.0);
                step++;
                break;

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

        }

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


Is there something thats not right.. changed the sensor to point to the right, and went to run, and i get the nxt screen saying i didnt set up one of the 3rd party sensors correctly.

Also. I am not able to use a touch sensor. I have the three sensor ports taken by the ir seeker, magnetic sensor, and the gyro.


Fri Apr 22, 2011 6:04 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1343
Post Re: Endcoders better?
Team2844 wrote:
Is there something thats not right.. changed the sensor to point to the right, and went to run, and i get the nxt screen saying i didnt set up one of the 3rd party sensors correctly.

Also. I am not able to use a touch sensor. I have the three sensor ports taken by the ir seeker, magnetic sensor, and the gyro.

Interesting... I am not sure who displayed that message, my code sure did not do that. In any case I think I forgot to tell you how to initialize the IR seeker sensor. First, you have to change the pragma line for the IR Seeker to the following, then in initializeRobot, you need to add the line to set the DSP mode although I thought it defaults to DSP_1200 already. But no harm to set it explicitly. Let me know if that works.
Code:
#pragma config(Sensor, S4,     IRSeeker2,           sensorLowSpeed)

void initializeRobot()
{
  HTIRS2setDSPMode(IRSeeker2, DSP_1200);
  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;
}


Fri Apr 22, 2011 6:28 pm
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: Endcoders better?
Where are you getting the sensorLowSpeed from?


Fri Apr 22, 2011 7:35 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1343
Post Re: Endcoders better?
That was from Xander's sample code. But if you want to pick from the list. Pick "sensorI2CCustom".


Fri Apr 22, 2011 7:36 pm
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: Endcoders better?
It runs the 12 inches.. stops.. then runs 56 inches just forward, ignoring the sensor. Is that where i need to tune it?
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,           sensorI2CCustom)
#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,     lights,        tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     motorArm,      tmotorNormal, openLoop, encoder)
#pragma config(Servo,  srvo_S1_C3_1,    trailer1,             tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    trailer2,             tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    gate,                 tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C3_5,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C3_6,    twist,                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:\Projects\rdpartyrobotcdr-v1.8.1-HT\drivers\HTIRS2-driver.h"

#define BOUND(n, l, h) (((n) < (l))? (l): ((n) > (h))? (h): (n))
#define PI                      3.14159
#define CLICKS_PER_REVOLUTION   1440.0          //encoder clicks per revolution
#define GEAR_RATIO              (40.0/120.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.005807764)
#define DEGREES_PER_CLICK       (0.03333)

GYRO  g_Gyro;
float g_turnTarget = 0.0;
bool  g_turnEnabled = false;
float g_turnTolerance = 0.3;    //needs to be tuned
float g_turnKp = 24.0;             //needs to be tuned
float g_driveTarget = 0.0;
bool  g_driveEnabled = false;
float g_driveTolerance = 0.5;
float g_driveKp = 50.0;
bool  g_driveIREnabled = false;
float g_turnIRKp = 10.0;    //need to tune this

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 (abs(error) > g_turnTolerance)
        {
            //
            // Simple proportional PID control.
            // Limit the outpout to the range of -70 to 70.
            //
            int turnPower = BOUND((int)(g_turnKp*error), -100, 100);
            motor[motorLeft] = turnPower;
            motor[motorRight] = -turnPower;
        }
        else
        {
            motor[motorLeft] = 0;
            motor[motorRight] = 0;
            g_turnEnabled = false;
            nxtDisplayTextLine(7, "err=%f", error);
        }
        nxtDisplayTextLine(3, "Heading=%f", GyroGetHeading(g_Gyro));
        nxtDisplayTextLine(6, "target=%f,tol=%f", g_turnTarget, g_turnTolerance);
    }
}

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 (abs(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), -100, 100);
            motor[motorLeft] = BOUND(drivePower + turnPower, -100, 100);
            motor[motorRight] = BOUND(drivePower - turnPower, -100, 100);
            nxtDisplayTextLine(7, "drvErr=%f", driveErr);
        }
        else
        {
            motor[motorLeft] = 0;
            motor[motorRight] = 0;
            g_driveEnabled = false;
        }
        nxtDisplayTextLine(4, "Distance=%f", GetDriveDistance());
   }
}

float IRGetACDir(tSensors link)
{
    static float prevDir = 0.0;
    float currDir;
    int acS[5];
    int idx;

    idx = HTIRS2readACDir(link);
    currDir = (float)idx;
    if (idx == 0)
    {
        currDir = prevDir;
    }
    else if (HTIRS2readAllACStrength(link, acS[0], acS[1], acS[2], acS[3], acS[4]))
    {
        idx = (idx - 1)/2;
        if ((idx < 4) && (acS[idx] != 0) && (acS[idx + 1] != 0))
        {
            currDir += (float)(acS[idx + 1] - acS[idx])/
                          max(acS[idx], acS[idx + 1]);
        }
    }
    prevDir = currDir;

    return currDir;
}

void SetDriveIRTarget(float distance)
{
    nMotorEncoder[motorLeft] = 0;
    nMotorEncoder[motorRight] = 0;
    g_driveTarget = distance;
    g_driveIREnabled = true;
}

void DriveIRTask()
{
    if (g_driveIREnabled)
    {
        float driveErr = g_driveTarget - GetDriveDistance();

        if (abs(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 = IRGetACDir(IRSeeker2) - 2.0;  //use zone 2 as the straight ahead zone
            int turnPower = BOUND((int)(g_turnIRKp*turnErr), -50, 50);
            motor[motorLeft] = BOUND(drivePower + turnPower, -100, 100);
            motor[motorRight] = BOUND(drivePower - turnPower, -100, 100);
        }
        else
        {
            motor[motorLeft] = 0;
            motor[motorRight] = 0;
            g_driveIREnabled = false;
        }
   }
}

void initializeRobot()
{
  HTIRS2setDSPMode(IRSeeker2, DSP_1200);
  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)
        {
//*****************************Drive to opp side of field*************************************

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

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

            case 2:
                // step 2: ir seeker drive.
                SetDriveIRTarget(56.0);
                step++;
                break;

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

        }

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


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