Hello, I made a PD program that does not work. The robot either barely moves or makes a turn. Any suggestions?
I forgot to post my code so here it is
// Robot Configuration:
// [Name] [Type] [Port(s)]
// VisionBack vision 7
// LineTrackerL line A
// LineTrackerM line B
// LineTrackerR line C
// RotationArm rotation 11
// GpsL gps 15
// Rotationrear rotation 20
// VisionFront vision 1
// ---- END VEXCODE CONFIGURED DEVICES ----
// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name] [Type] [Port(s)]
// VisionBack vision 7
// LineTrackerL line A
// LineTrackerM line B
// LineTrackerR line C
// RotationArm rotation 11
// GpsL gps 15
// Rotationrear rotation 20
// VisionFront vision 1
// ---- END VEXCODE CONFIGURED DEVICES ----
// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name] [Type] [Port(s)]
// VisionBack vision 7
// LineTrackerL line A
// LineTrackerM line B
// LineTrackerR line C
// RotationArm rotation 11
// GpsL gps 15
// Rotationrear rotation 20
// VisionFront vision 1
// ---- END VEXCODE CONFIGURED DEVICES ----
// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name] [Type] [Port(s)]
// VisionBack vision 7
// LineTrackerL line A
// LineTrackerM line B
// LineTrackerR line C
// RotationArm rotation 11
// GpsL gps 15
// Rotationrear rotation 18
// VisionFront vision 1
// ---- END VEXCODE CONFIGURED DEVICES ----
// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name] [Type] [Port(s)]
// VisionBack vision 7
// LineTrackerL line A
// LineTrackerM line B
// LineTrackerR line C
// RotationArm rotation 11
// GpsL gps 15
// Rotationrear rotation 18
// VisionFront vision 1
// ---- END VEXCODE CONFIGURED DEVICES ----
/*----------------------------------------------------------------------------*/
/* */
/* Module: main.cpp */
/* Author: VEX */
/* Created: Mon Aug 24 2540 */
/* Description: Rotation Sensing */
/* */
/* This program will demonstrate how to use Rotation Sensor commands */
/* to get information about the sensor's angle, position, and velocity */
/* */
/*----------------------------------------------------------------------------*/
#include "vex.h"
#include <vex_triport.h>
#include <vex_vision.h>
#include <cmath>
#include <iostream>
using namespace vex;
competition Competition;
brain brain1;
vex::motor LtMotorF = vex::motor(vex::PORT2);
vex::motor RtMotorF = vex::motor(vex::PORT4);
vex::motor LtMotorB = vex::motor(vex::PORT18,true);
vex::motor RtMotorB = vex::motor(vex::PORT5,true);
vex::motor Arm = vex::motor(vex::PORT19);
vex::motor Intake = vex::motor(vex::PORT17);
motor_group IntakeMotor( Arm, Intake);
vex::motor Claw = vex::motor(vex::PORT13);
vex::motor Rear = vex::motor(vex::PORT10);//top
motor_group LeftMotor (LtMotorB,LtMotorF);
motor_group RightMotor(RtMotorB,RtMotorF);
vex::controller Controller1 = vex::controller();
void pre_auton(void) {
// Initializing Robot Configuration. DO NOT REMOVE!
vexcodeInit();
// All activities that occur before the competition starts
// Example: clearing encoders, setting servo positions, ...
}
//Settings
double kP = 0.04;
double kD = 0.04;
double turnkP = 0.0;
double turnkI = 0.0;
double turnkD = 0.0;
bool resetDriveSensors = false;
//Variables modified for use
bool enableDrivePID = true;
int drivePID(int desiredValue, int desiredTurnValue){
int error; //SensorValue - DesiredValue : Position
int prevError = 0; //Position 20 miliseconds ago
int derivative; // error - prevError : Speed
int turnError; //SensorValue - DesiredValue : Position
int turnPrevError = 0; //Position 20 miliseconds ago
int turnDerivative; // error - prevError : Speed
LtMotorB.setPosition(0,degrees);
RtMotorB.setPosition(0,degrees);
while(LtMotorB.position(degrees)<desiredValue+desiredTurnValue){
//Get the position of both motors
int leftMotorPosition = LtMotorB.position(degrees);
int rightMotorPosition = RtMotorB.position(degrees);
///////////////////////////////////////////
//Lateral movement PID
/////////////////////////////////////////////////////////////////////
//Get average of the two motors
int averagePosition = (leftMotorPosition + rightMotorPosition)/2;
//Potential
error = averagePosition - desiredValue;
//Derivative
derivative = error - prevError;
double lateralMotorPower = 1.5*(error * kP + derivative * kD);
/////////////////////////////////////////////////////////////////////
///////////////////////////////////////////
//Turning movement PID
/////////////////////////////////////////////////////////////////////
//Get average of the two motors
int turnDifference = leftMotorPosition - rightMotorPosition;
//Potential
turnError = turnDifference - desiredTurnValue;
//Derivative
turnDerivative = turnError - turnPrevError;
double turnMotorPower = turnError * turnkP + turnDerivative * turnkD ;
/////////////////////////////////////////////////////////////////////
LeftMotor.spin(directionType::rev, lateralMotorPower+turnMotorPower, velocityUnits::pct);
RightMotor.spin(directionType::fwd,lateralMotorPower-turnMotorPower, velocityUnits::pct);
prevError = error;
turnPrevError = turnError;
vex::task::sleep(20);
}
LeftMotor.stop(brakeType::hold);
RightMotor.stop(brakeType::hold);
return 1;
}
/*---------------------------------------------------------------------------*/
/* */
/* Autonomous Task */
/* */
/* This task is used to control your robot during the autonomous phase of */
/* a VEX Competition. */
/* */
/* You must modify the code to add your own robot specific commands here. */
/*---------------------------------------------------------------------------*/
void autonomous(void) {
resetDriveSensors = true;
drivePID(3000,0);
}
/*---------------------------------------------------------------------------*/
/* */
/* User Control Task */
/* */
/* This task is used to control your robot during the user control phase of */
/* a VEX Competition. */
/* */
/* You must modify the code to add your own robot specific commands here. */
/*---------------------------------------------------------------------------*/
void usercontrol(void) {
enableDrivePID = false;
///////////////////////////
//Settings
///////////////////////////////////////////////////////////////////////////
//Drivetrain
double turnImportance = 0.5;
while (1) {
///////////////////////////
//Driver Control
///////////////////////////////////////////////////////////////////////////
double turnVal = Controller1.Axis1.position(percent);
double forwardVal = Controller1.Axis3.position(percent);
double turnVolts = turnVal * 0.12;
double forwardVolts = forwardVal * 0.12 * (1 - (std::abs(turnVolts)/12.0) * turnImportance);
//0 - 12 = -12
//0 + 12 = 12(due to cap)
LeftMotor.spin(forward, forwardVolts + turnVolts, voltageUnits::volt);
RightMotor.spin(forward, forwardVolts - turnVolts, voltageUnits::volt);
///////////////////////////////////////////////////////////////////////////
///////////////////////////
//Arm Control
///////////////////////////////////////////////////////////////////////////
bool topRightButton = Controller1.ButtonR1.pressing();
bool bottomRightButton = Controller1.ButtonR2.pressing();
if (topRightButton){
Arm.spin(forward, 12.0, voltageUnits::volt);
}
else if (bottomRightButton){
Arm.spin(forward, -12.0, voltageUnits::volt);
}
else{
Arm.spin(forward, 0, voltageUnits::volt);
}
///////////////////////////////////////////////////////////////////////////
///////////////////////////
//Claw Control
///////////////////////////////////////////////////////////////////////////
bool topLeftButton = Controller1.ButtonL1.pressing();
bool bottomLeftButton = Controller1.ButtonL2.pressing();
if (topLeftButton){
Claw.spin(forward, 12.0, voltageUnits::volt);
}
else if (bottomLeftButton){
Claw.spin(forward, -12.0, voltageUnits::volt);
}
else{
Claw.spin(forward, 0, voltageUnits::volt);
}
///////////////////////////////////////////////////////////////////////////
wait(20, msec); // Sleep the task for a short amount of time to
// prevent wasted resources.
}
}
//
// Main will set up the competition functions and callbacks.
//
int main() {
// Set up callbacks for autonomous and driver control periods.
resetDriveSensors = true;
drivePID(1000,0);
}
1 Like
A couple of suggestions:
- Walk thru your code and keep track of values each time thru the loop. Where you get the position from the motors, use a value you think they would report. Remember, 20ms is a pretty short amount of time
- Print out the values from the motors’ position and what you set the velocity to each time (or every 10 times) thru the loop
- Take a look at your type definitions for variables; you probably want to switch most (or all) of your variables from
inttodouble.
On the last point, understand that an int can only hold values of 0, 1, 2, etc. (or -1, -2, etc.), while double can hold 6.28318, 1.0, 0.0 etc.
5 Likes
This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.