Hello everyone!
As I am working on my robot, I have to think about how to program my robot in such a manner where the robot is almost capable to hold its own position without the need of additional elements. Basically speaking, PID is built in such a manner where it doesn’t take consideration of weight, but it is essential in going from point A to point B with the extra ability to react against external forces. So, what we need to do is create a simple convenient algorithm to negate weight to be able to have a PID algorithm work precise and conveniently.
First of all, we need to understand that in order to properly negate, we must be able to get the sensor value of an arm in degrees, not in potentiometer 12_bit or any other type of sensor value. Also, as an addition, we must consider the fact that the resting(zero) position must be facing upwards (or vertical).
Yellow = upAngle perpendicular to the floor (or up)
Blue = Arm at 30 degrees from perpendicular
Black = Angle reading after conversion
I don't know how to convert it
You should convert it using the equation below:
double upAngle = (sensorValue * x) - y;Where x is the multiplier to turn the value into degrees, and y is the offset. If the sensor value is already in degrees then just do sensorValue - y. For example:
double upAngle = (potentiometerValue * 0.064) -12;This would convert 12_bit potentiometer value into degrees, then the -12 would be the offset (in degrees).
Next, we are going to have to include the following to be able to do weight negation:
Make sure to add C++ math library for calculations on the top of the program below the other includes
#include <cmath>Make sure to add a function to convert degrees to radians, as cmath utilizes trig functions with radians
//Used to convert degrees to radians
double toRad(double degrees){
return ( degrees * M_PI ) / 180 ;
}After you include the function and the other includes, you can now properly apply the weight negation using the function below:
//Runs Weight-Negation
double weightNegation(double upAngle, double weight){
return (std::sin(toRad(upAngle)) * weight);
}The upAngle is the angle, in degrees, the arm is from being vertical.
So, how do I apply this function properly?
Its simple!
/*----------------------------------------------------------------------------*/
/* */
/* Module: main.cpp */
/* Author: C:\Users\Connor */
/* Created: Wed Jul 29 2020 */
/* Description: V5 project */
/* */
/*----------------------------------------------------------------------------*/
// ---- START VEXCODE CONFIGURED DEVICES ----
// ---- END VEXCODE CONFIGURED DEVICES ----
#include "vex.h"
#include <cmath>
//Settings
double weightMultiplier = 0.0;
double kP = 0.0;
double kI = 0.0;
double kD = 0.0;
using namespace vex;
//Used to convert degrees to radians
double toRad(double degrees){
return ( degrees * M_PI ) / 180 ;
}
double weightNegation(double upAngle, double weight){
return (std::sin(toRad(upAngle)) * weight);
}
int main() {
// Initializing Robot Configuration. DO NOT REMOVE!
vexcodeInit();
while(true){
//Insert Pid algorithm, you already know how to do it
double PIDPower = error * kP + totalError * kI + derivative * kD;
double upAngle = (lePot.value(analogUnits::range12bit) * 0.064) -12;
double negatePower = weightNegation(upAngle, weightMultiplier);
leMotor.spin(forward, PIDPower + negatePower,voltageUnits::volt);
vex::task::sleep(10);
}
}
How should I tune the arm with Weight Negation and PID?
- set kP, kI, kD, and weightMultiplier to 0.0
- Increase weightMultiplier until the arm can hold its position at any angle, and also if you push the arm up and down one doesn’t require more force than the other (Basically, feeling weightless)
Note: If increasing the weightMultiplier makes the arm motor push down instead of up, changeleMotor.spin(forward, PIDPower + negatePower,voltageUnits::volt);toleMotor.spin(forward, PIDPower - negatePower,voltageUnits::volt); - Tune kP, kI, and kD accordingly
