| Author |
Message |
|
MHTS
Guru
Joined: Sun Nov 15, 2009 5:46 am
Posts: 1023
|
 Re: Endcoders better? That's weird. Please add the following line to TurnTask:
|
| Thu Apr 14, 2011 2:18 am |
|
|
|
Team2844
Novice
Joined: Mon Oct 18, 2010 9:31 pm
Posts: 79
|
Re: Endcoders better? Heading:0.000000000
Distance:48.01467
Step 4
Target 90.000000000
err: -90.00000000
|
| Thu Apr 14, 2011 2:23 am |
|
|
|
MHTS
Guru
Joined: Sun Nov 15, 2009 5:46 am
Posts: 1023
|
Re: Endcoders better? Ah, my bad. Here is the fix:
|
| Thu Apr 14, 2011 2:27 am |
|
|
|
Team2844
Novice
Joined: Mon Oct 18, 2010 9:31 pm
Posts: 79
|
Re: Endcoders better? It went the 48 inches.... and then got stuck... the screen said
Distance:48.32453
Step 1
|
| Thu Apr 14, 2011 2:36 am |
|
|
|
MHTS
Guru
Joined: Sun Nov 15, 2009 5:46 am
Posts: 1023
|
Re: Endcoders better? What is your g_driveTolerance? Add the following lines to DriveTask:  | | | | Code: 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), -70, 70);
motor[motorLeft] = BOUND(drivePower + turnPower, -100, 100);
motor[motorRight] = BOUND(drivePower - turnPower, -100, 100);
nxtDisplayTextLine(7, "drvErr=%f", driveErr);
}
| | | | |
|
| Thu Apr 14, 2011 2:40 am |
|
|
|
Team2844
Novice
Joined: Mon Oct 18, 2010 9:31 pm
Posts: 79
|
Re: Endcoders better? After looking at it with the last error you had me put in. I found I had changed theg_drivetolerance to 0.0 on accident. Changed it and it turns the robot now.
Some new questions now. My robot has a main arm that is run my a Textric Motor with an Encoder. How do I add that into the code to make it work. Also Servos, any different than writing them in time seqence?
|
| Thu Apr 14, 2011 9:38 am |
|
|
|
MHTS
Guru
Joined: Sun Nov 15, 2009 5:46 am
Posts: 1023
|
Re: Endcoders better? Acoording to my understanding, the Tetrix servo on the NXT does not support feedback, meaning that if you set the servo to go to a certain angle, you won't know when it is completed. So there is no equivalent of encoder on servos that tells you the current position. Therefore, if you need to wait for the servo to get done, the only way is time. Fortunately, waiting for it to get done is usually not an accuracy issue so it is acceptable. It willl still go to the precise position you set it, you just won't know precisely when it is done. BTW, you should also rename the servos to something that make sense. Instead of servo1 or servo2, you should name them servoWrist, servoGrabber etc. Assuming you have a wrist servo, here is how you add time to it.
|
| Thu Apr 14, 2011 12:06 pm |
|
|
|
MHTS
Guru
Joined: Sun Nov 15, 2009 5:46 am
Posts: 1023
|
Re: Endcoders better? For moving the arm on a motor with encoder, you can do similar thing as the drive motors. BTW, the nice thing about the arrangement of the state machine is that you can do several things in parallel. Notice I can start the wrist and the arm at the same time. This is called cooperative multi-tasking. You can even do it at the same time as driving the 48 inches if you want.
|
| Thu Apr 14, 2011 12:32 pm |
|
|
|
Team2844
Novice
Joined: Mon Oct 18, 2010 9:31 pm
Posts: 79
|
Re: Endcoders better? Hello again. I had a few things going on other than robotics that had to come first, but I am still working on the coding. I put in the new coding up above, and Im going to test it later today. But I have a nother question. I have one Lego motor on the robot(as much as i hate them) but All I want to do is turn it on and off for an amount of time. How do I add that into this code.
Thanks
|
| Thu Apr 21, 2011 10:06 am |
|
|
|
MHTS
Guru
Joined: Sun Nov 15, 2009 5:46 am
Posts: 1023
|
Re: Endcoders better? It would be similar to timing the servo. The only difference is that servo will stop when it reaches target. In this case, you will have to turn off the lego motor at the end of the timer wait.
|
| Thu Apr 21, 2011 12:42 pm |
|
|
|
Team2844
Novice
Joined: Mon Oct 18, 2010 9:31 pm
Posts: 79
|
Re: Endcoders better? Ok, so here are a few codes... with problems that i am having with them. | | | | 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.5; //needs to be tuned
float g_turnKp = 50.0; //needs to be tuned
float g_driveTarget = 0.0;
bool g_driveEnabled = false;
float g_driveTolerance = 0.3;
float g_driveKp = 50.0;
int g_armTarget = 0;
bool g_armEnabled = false;
int g_armTolerance = 0;
float g_armKp = 1.0;
void SetArmTarget(int clicks)
{
g_armTarget = clicks;
g_armEnabled = true;
}
void ArmTask()
{
if (g_armEnabled)
{
int error = g_armTarget - nMotorEncoder[motorArm];
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.
while (true)
{
GyroTask(g_Gyro);
nxtDisplayTextLine(5, "Step=%d", step);
switch (step)
{
//*****************************Drive to trailer*************************************
case 0:
// step 0: go forward.
SetArmTarget(-10);
ClearTimer(T1);
step++;
break;
case 1:
// step 3: wait to complete.
if ((time1[T1] > 500)&& (g_armEnabled == false))
{
step++;
}
break;
}
ArmTask();
wait1Msec(10);
}
} | | | | |
I am running this code to just bring the arm out. My robot starts with its arm straight up, and then come down when i need to. When I run this... the arm motor keeps running, and doesn't stop at the said clicks.. i have tried different clicks, from 10, , -1, -10, -100, -200, and so on, and it still wont stop after the clicks. On to the other 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:\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 = 30.0; //needs to be tuned
float g_driveTarget = 0.0;
bool g_driveEnabled = false;
float g_driveTolerance = 0.3;
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 trailer*************************************
case 0:
// step 0: go forward.
SetDriveTarget(28.0);
step++;
break;
case 1:
// step 1: wait for drive to complete.
if (g_turnEnabled == false)
{
step++;
}
break;
case 2:
// step 2: turn 90 right degrees.
SetTurnTarget(-90.0);
step++;
break;
case 3:
// step 3: wait for drive to complete.
if (g_turnEnabled == false)
{
step++;
}
break;
case 4:
// step 2: drive forward to trailer.
SetDriveTarget(30);
step++;
break;
case 5:
// step 3: wait for drive to complete.
if (g_turnEnabled == false)
{
step++;
}
break;
//********BATons dispence******
case 6:
// motorA to turn on.
motor[motorA] = 95;
ClearTimer(T1);
step++;
break;
case 7:
// step 3: wait to complete.
if (time1[T1] > 4000)
{
motor[motorA] = 0;
step++;
}
break;
//*************************Running to the other side of the field************************8
case 8:
// step 10: drive forward, pushing trailer.
SetDriveTarget(39.0);
step++;
break;
case 9:
// step 11: wait for drive to complete.
if (g_turnEnabled == false)
{
step++;
}
break;
case 10:
// step 2: turn 90 left degrees to go over the mountain.
SetTurnTarget(90.0);
step++;
break;
case 11:
// step 3: wait for drive to complete.
if (g_turnEnabled == false)
{
step++;
}
break;
// stopped here because Im working on half a field, trying to get this down first before I go farther.
}
TurnTask();
DriveTask();
wait1Msec(10);
}
} | | | | |
I have been working on this code for going to the trailer(leaving the arm movement out because i cant get it to work) lining up with the trailer, and then driving to the next point, which is going to the other side of the field. But when I run this, it goes through the first drive forward, turn right 90, but then when it goes to run the 30 inches to the trailer, motorA(lego motor) starts to run, when in the code, says to run after the 30 inches is reached. It should then turn off after the 4 seconds, and start to move to over the mountain, but motor A keeps running till it turns 90 degrees to go over the mountain.
|
| Fri Apr 22, 2011 12:49 am |
|
|
|
MHTS
Guru
Joined: Sun Nov 15, 2009 5:46 am
Posts: 1023
|
Re: Endcoders better? | | | | Team2844 wrote: I am running this code to just bring the arm out. My robot starts with its arm straight up, and then come down when i need to. When I run this... the arm motor keeps running, and doesn't stop at the said clicks.. i have tried different clicks, from 10, , -1, -10, -100, -200, and so on, and it still wont stop after the clicks.
| | | | |
For this problem, I spotted several issues. First, your arm tolerance is zero. Like I said before, although PID control is precise, it is impossible to be that exact, so you may want to give it a little tolerance. Regarding the arm motor not stopping, there are a few possibilities. To debug this, use the same technique I showed you before. In the ArmTask add the following display line. You may want to remove other debug lines to free a line up if you are running out of lines on the display. Also, whenever you add new code with PID control, you need to tune their Kp and tolerance individually. I don't know the armKp being 1.0 is correct. Don't use the values I randomly put in. You need to tune them. Is your arm movement linear or is it turning an angle? You may want to translate the clicks into something more meaningful. For example, if it is angle, assuming the tetrix motor is driving the arm directly without gears, you may add the following code. DEGREES_PER_CLICK is probably 0.25 (The encoder is 1440 clicks per revolution). If the arm has gears, then you need to factor in the gear ratio. Also, unlike the turn target and turn task which is relative (turn 90 degrees relative to you current position), the arm target should be an absolute angle. So you need to move your arm to the "zero position" before you run the program and you need to add a line in initializeRobot to reset the arm encoder.
|
| Fri Apr 22, 2011 8:25 am |
|
|
|
MHTS
Guru
Joined: Sun Nov 15, 2009 5:46 am
Posts: 1023
|
Re: Endcoders better? | | | | Team2844 wrote: I have been working on this code for going to the trailer(leaving the arm movement out because i cant get it to work) lining up with the trailer, and then driving to the next point, which is going to the other side of the field. But when I run this, it goes through the first drive forward, turn right 90, but then when it goes to run the 30 inches to the trailer, motorA(lego motor) starts to run, when in the code, says to run after the 30 inches is reached. It should then turn off after the 4 seconds, and start to move to over the mountain, but motor A keeps running till it turns 90 degrees to go over the mountain. | | | | |
For this problem, I spotted many issues. First, in step 0, you go forward 28 inches but in step 1, you are waiting for turn to complete??? You should wait for drive to complete (i.e. if (g_driveEnabled == false)). In step 3, the code is correct but the comment is wrong. It should say "wait for turn to complete". Again in step 5, the code is wrong, it should be checking g_driveEnabled instead of g_turnEnabled (the comment step number is wrong too). If the lego motor is the baton dispenser, you should rename motorA to motorDispense. If the dispense motor did not stop at step 7 then add the following code to debug it: Again, you are checking for the wrong thing in step 9, you should wait for g_driveEnabled instead. In general, be mindful when you cut and paste code. Make sure the code is modified to what you actually meant.
|
| Fri Apr 22, 2011 9:12 am |
|
|
|
Team2844
Novice
Joined: Mon Oct 18, 2010 9:31 pm
Posts: 79
|
Re: Endcoders better? For the arm issue. I have done everything that you said on the arm, but when I start it... the encoder starts at the -45 and goes farther into the negatives when it runs. I told it to zero the encoder out though... did i put something in the wrong place?
|
| Fri Apr 22, 2011 10:13 am |
|
|
|
MHTS
Guru
Joined: Sun Nov 15, 2009 5:46 am
Posts: 1023
|
Re: Endcoders better? The arm could have moved after the initialization possibly due to human handling of the robot or just gravity. So starting at -45 may not be uncommon. To minimize this, you could take the zeroing of the encoder out of initializeRobot and put it after waitForStart but before entering the forever-while loop. Regarding the "going into negative", that depends on how you wire the motor. Since you have to set target to -45, your arm motor is wired backward. That's ok, you just need to reverse the check. BTW, what is your DEGREES_PER_CLICK? Is the value correct on your robot? Also, what are your Kp and tolerance?
|
| Fri Apr 22, 2011 1:08 pm |
|
|
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
|
|
