Hello there! I just wrote a pid code but idk how to start this code. For example, in this pid code, how can i set my pid code for going 30 inches forward in autonomous process?
This is my code:
/----------------------------------------------------------------------------/
/* /
/ Module: main.cpp /
/ Author: VEX /
/ Created: Thu Sep 26 2019 /
/ Description: Competition Template /
/ /
/----------------------------------------------------------------------------*/
// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name] [Type] [Port(s)]
// InertialSensor inertial 19
// ElevatorLift motor 10
// RFM motor 15
// RBM motor 16
// LFM motor 13
// LBM motor 14
// Controller1 controller
// BackLift motor 4
// ArmGroup motor_group 1, 2
// digoutG digital_out G
// digoutH digital_out H
// digoutA digital_out A
// ---- END VEXCODE CONFIGURED DEVICES ----
#include "vex.h"
#include <cmath>
using namespace vex;
// A global instance of competition
competition Competition;
// define your global instances of motors and other devices here
/*---------------------------------------------------------------------------*/
/* Pre-Autonomous Functions */
/* */
/* You may want to perform some actions before the competition starts. */
/* Do them in the following function. You must return from this function */
/* or the autonomous and usercontrol tasks will not be started. This */
/* function is only called once after the V5 has been powered on and */
/* not every time that the robot is disabled. */
/*---------------------------------------------------------------------------*/
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, ...
}
/*---------------------------------------------------------------------------*/
/* */
/* 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. */
/*---------------------------------------------------------------------------*/
//constant for pid true/false
bool enableDrivePID = true;
//Movement PID Settings
double kP = 0.3;
double kD = 0.1;
//Turn PID Settings
double turnkI = 0.2;
double turnkP = 0.5;
double turnkD = 0.1;
//Autonomous Settings
int desiredValue = InertialSensor.rotation(degrees);
int desiredTurnValue = 0;
//Movement Errors
int error; // Sensor Value - Desired Value --> Positional Value -> speed -> acceleration
int prevError = 0; // Position 20 miliseconds ago
int derivative; // Error - prevError : speed(velocity)
int totalError = 0; // totalError = totalError + error;
//Turning Errors
int turnError; // Sensor Value - Desired Value --> Positional Value -> speed -> acceleration
int turnPrevError = 0; // Position 20 miliseconds ago
int turnDerivative; // Err or - prevError : speed(velocity)
int turnTotalError = 0; // totalError = totalError + error;
//reset drive sensors
bool resetDriveSensors = false;
int drivePID(){
while(enableDrivePID){
if(resetDriveSensors){
resetDriveSensors = false;
LBM.setPosition(0,degrees);
LFM.setPosition(0,degrees);
RBM.setPosition(0,degrees);
RBM.setPosition(0,degrees);
}
//getting positions of motors
int lfmP = LFM.position(degrees);
int lbmP = LBM.position(degrees);
int rfmP = RFM.position(degrees);
int rbmP = RBM.position(degrees);
//////////////////////////////////////////////
///////Lateral Motor Power
//////////////////////////////////////////////
//getting average position
int averagePosition = (lfmP+lbmP+rfmP+rbmP)/4;
//Potential
error = desiredValue - averagePosition;
//Derivative
derivative = error - prevError;
//calculating motor power
double lateralMotorPower = (error * kP + derivative * kD);
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////
///////Lateral Motor Power
//////////////////////////////////////////////
//////////////////////////////////////////////
///////Turning Motor Power
//////////////////////////////////////////////
//getting average position
int turnDifference = (lfmP+lbmP)-(rfmP+rbmP);
//Potential
turnError = desiredTurnValue - turnDifference;
//Derivative
turnDerivative = turnError - turnPrevError;
//calculating motor power
double turnMotorPower = (turnError * turnkP + turnDerivative * turnkD);
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//start motors
LFM.spin(forward,lateralMotorPower + turnMotorPower,volt);
LBM.spin(forward,lateralMotorPower + turnMotorPower,volt);
RFM.spin(forward,lateralMotorPower - turnMotorPower,volt);
RBM.spin(forward,lateralMotorPower - turnMotorPower,volt);
//code
prevError = error;
turnPrevError = turnError;
vex::task::sleep(20);
}
return 1;
}
void autonomous(void) {
vex::task start(drivePID);
resetDriveSensors = true;
desiredTurnValue = 200;
}
/*---------------------------------------------------------------------------*/
/* */
/* 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. */
/*---------------------------------------------------------------------------*/
int counter=0;
int counter2=1;
int counter3 = 0;
int counter4 = 0;
void pressedFunc(){
if(counter==0){
counter=1;
}
else{
counter=0;
}
}
void pressedFunc2(){
if(counter2==1){
counter2=0;
}
else{
counter2=1;
}
}
void pressedFunc3(){
counter3=1;
}
void pressedFunc4(){
counter4=1;
}
void usercontrol(void) {
// User control code here, inside the loop
enableDrivePID = false;
//Settings
double turnImportance = 0.5;
Controller1.ButtonLeft.pressed(pressedFunc);
Controller1.ButtonRight.pressed(pressedFunc2);
Controller1.ButtonR1.pressed(pressedFunc3);
Controller1.ButtonR2.pressed(pressedFunc4);
while(1){
if(Controller1.Axis3.position(pct) != 0){
LFM.spin(reverse,Controller1.Axis3.position(),pct);
LBM.spin(reverse,Controller1.Axis3.position(),pct);
RFM.spin(forward,Controller1.Axis3.position(),pct);
RBM.spin(forward,Controller1.Axis3.position(),pct);
if(Controller1.Axis1.position(pct) != 0){
LFM.spin(reverse,Controller1.Axis1.position(),pct);
LBM.spin(reverse,Controller1.Axis1.position(),pct);
RFM.spin(reverse,Controller1.Axis1.position(),pct);
RBM.spin(reverse,Controller1.Axis1.position(),pct);
}
}
else if(Controller1.Axis1.position(pct) != 0){
LFM.spin(reverse,Controller1.Axis1.position(),pct);
LBM.spin(reverse,Controller1.Axis1.position(),pct);
RFM.spin(reverse,Controller1.Axis1.position(),pct);
RBM.spin(reverse,Controller1.Axis1.position(),pct);
if(Controller1.Axis3.position(pct) != 0){
LFM.spin(reverse,Controller1.Axis3.position(),pct);
LBM.spin(reverse,Controller1.Axis3.position(),pct);
RFM.spin(forward,Controller1.Axis3.position(),pct);
RBM.spin(forward,Controller1.Axis3.position(),pct);}
}
else{
LFM.stop(brake);
LBM.stop(brake);
RFM.stop(brake);
RBM.stop(brake);
}
bool topRightButton = Controller1.ButtonL1.pressing();
bool bottomRightButton = Controller1.ButtonL2.pressing();
if(topRightButton){
ArmGroup.spin(forward,7.0,volt);
}
else if(bottomRightButton){
ArmGroup.spin(forward,-6.0,volt);
}
else{
ArmGroup.stop(brakeType::brake);
}
bool topLeftButton = Controller1.ButtonUp.pressing();
bool bottomLeftButton = Controller1.ButtonDown.pressing();
if(topLeftButton){
BackLift.spin(forward,12.0,volt);
}
else if(bottomLeftButton){
BackLift.spin(forward,-12.0,volt);
}
else{
BackLift.stop(brakeType::brake);
}
if(Controller1.ButtonX.pressing()){
ElevatorLift.spin(forward,12.0,volt);
}
else if(Controller1.ButtonB.pressing()){
ElevatorLift.spin(forward,-12.0,volt);
}
else{
ElevatorLift.stop(brakeType::brake);
}
if(counter==1){
digoutG.set(true);
}
else if(counter==0){
digoutG.set(false);
}
if(counter2==0){
digoutH.set(true);
}
else if(counter2==1){
digoutH.set(false);
}
if(counter3==1){
digoutA.set(true);
}
if(counter4==1){
digoutA.set(false);
}
}
}
//
// Main will set up the competition functions and callbacks.
//
int main() {
// Set up callbacks for autonomous and driver control periods.
Competition.autonomous(autonomous);
Competition.drivercontrol(usercontrol);
// Run the pre-autonomous function.
pre_auton();
// Prevent main from exiting with an infinite loop.
while (true) {
wait(100, msec);
}
}