Hi ,we have a bit of a problem ,was hoping for some help or suggestions …
So this year we are using 6 wheels (instead of the mecanum which we used in previous years)…When programming I used the prepared function that first provided for TankDrive mod , so when we ran the code it didn’t function as expected (we are using Plaction Wheels ) …
When moving on the carpet it gets stuck while curves ,however it worked ok on plastic floor,we know that on the carpet friction between the wheels and the carpet is higher than the plastic ,and this may be partially the problem ….my question is ,can I (by programming) overcome the friction issue ,by tampering the speed given to the motors ??? I am open to any ideas ….
and here is the code :
#include "WPILib.h"
#include "Math.h"
#include "Vision/AxisCamera.h"
#include"C:\Users\MF\Documents\Vision/AxisCameraParams.h"
#define GRAY_AREA 0.06 //The area that human hand may commit an error in .
#define GRAY_AREA_SHOOT 0.08
#define MIN_SPEED 0.1 //the min speed that the wheels may get while turning.
#define INVERT -1 //invertieng a positive value to a neg value.
#define ANGLE2SPEED 0.06 // to convert Gyro angles to motor speed since angle is -30 to 30 and speed from -1 to 1
#define MAXSPEEDGYRO 0.4 // max speed in gyro mood before inverting correction from left to right
#define PI 3.14159
#define RAMP 0.5
#define BALLUP 0.9 // The Speed of lefting balls motors.
#define SHOOTER3 -.92
#define SHOOTER2 -0.76
#define SHOOTER1 -0.6
//#define SETANGLE 0.5// The speed of setting an angle.
//Constants which define the valid arm positions
#define ARM_MAX_ANGLE 105
#define ARM_MIN_ANGLE -90
#define Z_PLANE_TOLERANCE 0.3 /* In meters */
//Constants which define the "trigger" angles for the various buttons
#define LEG_FORWARD -110
#define LEG_BACKWARD -80
#define LEG_OUT -65
#define HEAD_LEFT 98
#define HEAD_RIGHT 70
#define SHOOTERK 0.7
/**
* This is a 2012 ReboundRumble program showing the use of mishka monster Robot.
* The SimpleRobot class is the base of a robot application that will automatically call your
* Autonomous and OperatorControl methods at the right time as controlled by the switches on
* the driver station or the field controls.
* the programe containes an OperatorControl mood where it containes the macnume wheels logratem and the arm logaretem
* also it containes the
* It contain the programming of a motor used to push the Ramp down .
* It also conatins the programming of a Conveyor Belt used to transmitt balls upward and downward as well.
*
*
*/
class RobotDemo: public SimpleRobot {
//RobotDrive myRobot; // robot drive system
//Joystick stick; // only joystick
Jaguar FL;//FrontLeft motor.
Jaguar FR;//FrontRight motor.
//Jaguar RL;//RearLeft motor.
//Jaguar RR;//RearRight motor.
Jaguar Ramp;
Jaguar BallUp1;
Jaguar Shooter;
RobotDrive myRobot; // Robot drive system
Joystick leftStick; // only joystick
Joystick rightStick;
Joystick Leftstick; // shooter+hangr stick
DriverStation *ds; // driver station object for getting selections
Gyro myGyro;// This is the Gyro sensor(balancing the Robot).
Joystick stick; // only joystick for the kinect
Kinect *kinect;
//Jaguar BallUp2;
//Jaguar SetAngle;
public:
RobotDemo(void) :
FL(1), FR(2),/** RL(3), RR(4),*/ myRobot(&FL, &FR /**, &RL, &RR*/), // These must be initialized in the same order.
leftStick(1),
rightStick(2),// as they are declared above.
Leftstick(3), myGyro(1), // as they are declared above.
Ramp(5),
BallUp1(6),
Shooter(8),
stick(1)
//BallUp2(7),
//SetAngle(8)
{
myRobot.SetExpiration(0.1);
AxisCamera& camera = AxisCamera::GetInstance();
kinect = Kinect::GetInstance();
}
/**
* Function to process a pair of joints into an angle in the XY plane. The joints are projected into
* the XY plane of the Kinect, then an angle is calculated using one joint as the origin and the
* X-axis as the reference. The X-axis can optionally be mirrored in order to avoid the discontinuity
* of the atan2 function.
* @param origin The joint to use as the origin for the calculation
* @param measured The second point to use as for the angle calculation
* @param mirrored Whether to mirror the X-axis or not
* @return Angle in degrees referenced from the X-axis
*/
double AngleXY(Skeleton::Joint origin, Skeleton::Joint measured,
UINT8 mirrored) {
return (atan2((measured.y - origin.y),
(mirrored) ? (origin.x - measured.x) : (measured.x - origin.x))
* 180 / PI);
}
/**
* Funtion to process a pair of joints into an angle in the YZ plane. The joint are projected into
* the YZ plane of the Kinect, than an angle is calculated using one joint as the origin and the
* Z-axis as the reference.The Z-axis can optionally be mirrored in order to avoid the discontinuity
* of the atan2 function.
* @param origin The joint to use as the origin for the angle calculation
* @param measured The second point to use as for the angle calculation
* @param mirrored Whether to mirror the Z-axis or not
* @return Angle in degrees referenced from the Z-axis
*/
double AngleYZ(Skeleton::Joint origin, Skeleton::Joint measured,
UINT8 mirrored) {
return (atan2((measured.y - origin.y),
(mirrored) ? (origin.z - measured.z) : (measured.z - origin.z))
* 180 / PI);
}
/**
* Function to determine whether or not two joints are in approximately the same XY plane
* IE. If they have approximately the same z coordinates
* @param origin The first joint to be used in the comparison
* @param measured The second joint to be used in the comparison
* @return Whether or not the joints are in approximately the same XY plane
*/
bool InSameZPlane(Skeleton::Joint origin, Skeleton::Joint measured,
double tolerance) {
return fabs(measured.z - origin.z) < tolerance;
}
/**
* Converts an input value in the given input range into an output value along the given
* output range.
* If the result would be outside of the given output range, it is constrained to the
* output range.
* @param input An input value within the given input range.
* @param inputMin The minimum expected input value.
* @param inputMax The maximum expected input value.
* @param outputMin The minimum expected output value.
* @param outputMax The maximum expected output value.
* @return An output value within the given output range proportional to the input.
*/
double CoerceToRange(double input, double inputMin, double inputMax,
double outputMin, double outputMax) {
// Determine the center of the input range
double inputCenter = fabs(inputMax - inputMin) / 2 + inputMin;
double outputCenter = fabs(outputMax - outputMin) / 2 + outputMin;
// Scale the input range to the output range
double scale = (outputMax - outputMin) / (inputMax - inputMin);
// Apply the transformation
double result = (input + -inputCenter) * scale + outputCenter;
// Constrain to the result range
return max(min(result, outputMax), outputMin);
}
/**
This is the Autonomous mode code,in this code we programmed a line tracker (photoSwitch),
we mounted 3 sensors on the robot with 1.5 inches apart.
It tracks the middle sensor(010) ,and corrects the robot's movement to follow the line
smoothly.
There are 8 possible cases,in each one the robot reacts appropriately to resume following
the track.
*/
void Autonomous(void) {
double leftAxis, rightAxis;
double leftAngle, rightAngle, headAngle, rightLegAngle, leftLegAngle,
rightLegYZ, leftLegYZ = 0;
bool dataWithinExpectedRange;
bool buttons[8];
while (IsAutonomous()) {
/* Only process data if skeleton is tracked */
if (kinect->GetTrackingState() == Kinect::kTracked) {
/* Determine angle of each arm and map to range -1,1 */
leftAngle = AngleXY(kinect->GetSkeleton().GetShoulderLeft(),
kinect->GetSkeleton().GetWristLeft(), true);
rightAngle = AngleXY(kinect->GetSkeleton().GetShoulderRight(),
kinect->GetSkeleton().GetWristRight(), false);
leftAxis = CoerceToRange(leftAngle, -70, 70, -1, 1);
rightAxis = CoerceToRange(rightAngle, -70, 70, -1, 1);
/* Check if arms are within valid range and at approximately the same z-value */
dataWithinExpectedRange = (leftAngle < ARM_MAX_ANGLE)
&& (leftAngle > ARM_MIN_ANGLE) && (rightAngle
< ARM_MAX_ANGLE) && (rightAngle > ARM_MIN_ANGLE);
dataWithinExpectedRange = dataWithinExpectedRange
&& InSameZPlane(
kinect->GetSkeleton().GetShoulderLeft(),
kinect->GetSkeleton().GetWristLeft(),
Z_PLANE_TOLERANCE) && InSameZPlane(
kinect->GetSkeleton().GetShoulderRight(),
kinect->GetSkeleton().GetWristRight(),
Z_PLANE_TOLERANCE);
/* Determine the head angle and use it to set the Head buttons */
headAngle = AngleXY(kinect->GetSkeleton().GetShoulderCenter(),
kinect->GetSkeleton().GetHead(), false);
bool button0 = headAngle > HEAD_LEFT;
bool button1 = headAngle < HEAD_RIGHT;
/* Calculate the leg angles in the XY plane and use them to set the Leg Out buttons */
leftLegAngle = AngleXY(kinect->GetSkeleton().GetHipLeft(),
kinect->GetSkeleton().GetAnkleLeft(), true);
rightLegAngle = AngleXY(kinect->GetSkeleton().GetHipRight(),
kinect->GetSkeleton().GetAnkleRight(), false);
bool button2 = leftLegAngle > LEG_OUT;
bool button3 = rightLegAngle > LEG_OUT;
/* Calculate the leg angle in the YZ plane and use them to set the Leg Forward and Leg Back buttons */
leftLegYZ = AngleYZ(kinect->GetSkeleton().GetHipLeft(),
kinect->GetSkeleton().GetAnkleLeft(), false);
rightLegYZ = AngleYZ(kinect->GetSkeleton().GetHipRight(),
kinect->GetSkeleton().GetAnkleRight(), false);
buttons[4] = rightLegYZ < LEG_FORWARD;
buttons[5] = rightLegYZ > LEG_BACKWARD;
buttons[6] = rightLegYZ < LEG_FORWARD;
buttons[7] = rightLegYZ > LEG_BACKWARD;
if (dataWithinExpectedRange) {
/**
* Drives using the Kinect axes scaled to 0.6 power
* Axes are inverted so arms up == joystick pushed away from you
*/
FL.Set(leftAxis*.6);
//RL.Set(leftAxis*.6);
//RR.Set(-rightAxis*.6);
FR.Set(-rightAxis*.6);
if (button2) {
BallUp1.Set(BALLUP);
FL.Set(0.0);
//RL.Set(0.0);
//RR.Set(0.0);
FR.Set(0.0);
}
else {
BallUp1.Set(0);
//BallUp2.Set(0);
}
if(button1){
//BallUp1.Set(-BALLUP);
Shooter.Set(SHOOTER1);
FL.Set(0.0);
//RL.Set(0.0);
//RR.Set(0.0);
FR.Set(0.0);
BallUp1.Set(0.0);
//if (leftLegAngle){
// SetAngle.Set(SETANGLE);
//}
}
/**
* Do something with boolean "buttons" here
*/
/* Optional SmartDashboard display of Kinect values */
//dash->PutDouble("Left Arm", -leftAxis);
//dash->PutDouble("Right Arm", -rightAxis);
//dash->PutBoolean("Head Left", buttons[0]);
//dash->PutBoolean("Head Right", buttons[1]);
//...etc...
} else {
/* Arms are outside valid range */
FL.Set(0.0);
//RL.Set(0.0);
//RR.Set(0.0);
FR.Set(0.0);
BallUp1.Set(0.0);
Shooter.Set(0.0);
/**
* Do default behavior with boolean "buttons" here
*/
/* Optional SmartDashboard display of Kinect values */
//dash->PutDouble("Left Arm", 0);
//dash->PutDouble("Right Arm", 0);
//dash->PutBoolean("Head Left", false);
//dash->PutBoolean("Head Right", false);
//...etc...
}
} else {
/* Skeleton not tracked */
FL.Set(0.0);
//RL.Set(0.0);
//RR.Set(0.0);
FR.Set(0.0);
BallUp1.Set(0.0);
Shooter.Set(0.0);
/**
* Do default behavior with boolean "buttons" here
*/
/* Optional SmartDashboard display of Kinect values */
//dash->PutDouble("Left Arm", 0);
//dash->PutDouble("Right Arm", 0);
//dash->PutBoolean("Head Left", false);
//dash->PutBoolean("Head Right", false);
//...etc...
}
Wait(0.01);
}
}
/**
* This function is called once each time the robot enters operator control.
*/
/**
* Runs the motors with arcade steering.
*/
void OperatorControl(void)
{
myRobot.SetSafetyEnabled(true);
while (IsOperatorControl())
{
bool Ramp_Trig =Leftstick.GetRawButton(2);
if (Ramp_Trig){
float RampSpeed = Leftstick.GetX();
if (RampSpeed > 0) {
Ramp.Set(RampSpeed);
} else if (RampSpeed<0){
Ramp.Set(RampSpeed);
}
}
else {
Ramp.Set(0.0);
}
//****************************************************************************************************
float Ballup = Leftstick.GetY();
if (Ballup < -GRAY_AREA_SHOOT){ // left arm up
BallUp1.Set(Ballup);
}else if (Ballup >GRAY_AREA_SHOOT){
BallUp1.Set(Ballup);
}else{
BallUp1.Set(0);
}
//*****************************************************************************************************
bool ShootBall1 = Leftstick.GetRawButton(1);
bool ShootBall2 = Leftstick.GetRawButton(4);
bool ShootBall3 = Leftstick.GetRawButton(3);
if(ShootBall1){
Shooter.Set( SHOOTER1);
} else if (ShootBall2)
{
Shooter.Set( SHOOTER2);
}
else if (ShootBall3){
Shooter.Set( SHOOTER3);
}
else {
Shooter.Set(0.0);
}
myRobot.TankDrive(leftStick,rightStick); // drive with arcade style (use right stick)
Wait(0.005); // wait for a motor update time
}
}
};
START_ROBOT_CLASS(RobotDemo)
;
IMG_1527.JPG
