I decided to start working on my autonomous and I have two functions, one to move the bot forward/back and another to move the arm up/down. I want to have them both run at the same time. How would I do that?
1 Like
what compiler are you using? RobotC?
with any compiler you can use a âstate machineâ approach where you sit in a loop and basically run two functions sequentially and continually.
If you are using RobotC you could make use of âtasksâ
Example(If using ROBOTC):
StartTask(Function1);
StartTask(Function2);
wait1msec(1000);
StopTask(Function1);
StopTask(Function2);
If you canât I would recommend rewriting them to be less strict in their use. The following example is both sudo code and for an tank/h drive.
function moveForward() {
wheel1speed = 127;
wheel2speed = 127;
wheel3speed = 127;
wheel4speed = 127;
}
function startLift() {
liftMotor1speed = 127;
liftMotor2speed = 127;
liftMotor3speed = 127;
liftMotor4speed = 127;
}
//autonomous task
startLift();
moveForward();
/*
both will begin basically at the same time then you could stop them with separate functions later in the code. This allows for equal start times then separate end times if desired
*/
I do not recommend multitasking as a solution to this even though it seems like the perfect solution to this particular problem. See this for more information: ROBOTC
edit: comment clarification
The reason I would multi-task is because if you are starting to do autonomous you will eventually need some type of PID to do it well for certain things (like lift height). Yes, do things sequentially when they make sense, but you will quickly end up with multi-task control loops for motion and such.
I have some examples of tasks in my PROS code
PROS - Click me
If you have any questions about it, message me or @hotel (hotel would most likely be the better option)
If you wanted to use tasks just do a task for drive and a task for your subsystem control loops. Feed the subsystem control task with asynchronous calls from the primary task and if you need feedback, use competition flags.
You can do this sort of thing:
void liftSet(amount) {
...
flag = false;
}
void liftTask() {
while(true) {
....
if(lift_position == desired_position) flag = true;
}
}
bool liftReady() {
return flag;
}
and in autonomous:
startTask(liftTask); //Lift control loop on its own task
liftSet(0.5); //Set loop target
drive(100, 127); //At the same time, complete a drive operation
while(!liftReady()); //delay while lift is working, you don't want to turn till its ready
turn(90, 127); //Do a turn only when the lift operation is complete
But remember, you want to minimize the number of tasks you create (1-2 is reasonable). Also, you probably want to go for a single task solution when possible. A multitask solution should only be used in a few situations such as what @TriDragon said with PID loops (like I used for this example).
PROS has more task management parameters than RobotC, so if you really want to optimize tasks, you can try that.
#pragma config(Sensor, in1, PotentiometerSensor, sensorPotentiometer)
#pragma config(Sensor, dgtl1, EncoderSensor, sensorQuadEncoder)
#pragma config(Motor, port2, leftFrontBase, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port3, leftBackBase, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port4, rightFrontBase, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port5, rightBackBase, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port6, liftMotor1, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port7, liftMotor2, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port8, liftMotor3, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port9, liftMotor4, tmotorVex393_MC29, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
task main() //Autonomous Code for a drive with 4 motors and a lift with 4 motors
{
//Have base drive until a certain point (Encoder Sensor involved with this)
while(SensorValue[EncoderSensor] < /*Some value but I'll use whatever*/ 100)
{
motor[leftFrontBase] = /*Some value but I'll use whatever*/ 120;
motor[leftBackBase] = /*Some value but I'll use whatever*/ 120;
motor[rightFrontBase] = /*Some value but I'll use whatever*/ 120;
motor[rightBackBase] = /*Some value but I'll use whatever*/ 120;
//Have lift go up until a certain point (Potentiometer Sensor involved with this)
while(SensorValue[PotentiometerSensor] < /*Some value but I'll use whatever*/ 234)
{
motor[liftMotor1] = /*Some value but I'll use whatever*/ 120;
motor[liftMotor2] = /*Some value but I'll use whatever*/ 120;
motor[liftMotor3] = /*Some value but I'll use whatever*/ 120;
motor[liftMotor4] = /*Some value but I'll use whatever*/ 120;
}
}
//You can have it where the base code is the while loop inside instead of the lift code
//Which ever one is on the outside is the 'master' code
//As soon as the 'master' code is done looping, the inside while loop will stop with it
//Hope you understand. If it's wrong then please tell me. I haven't had time to test it.
//I have a feeling it might be wrong. If it's not then please tell me.
}
Wait⌠The code above wonât work since the lift while loop will keep running until itâs done. Then the whole âmasterâ loop will run again, which will run the lift while loop, but that has already been done since it looped already. Did that make sense? Just know that it wonât work!
#pragma config(Sensor, in1, PotentiometerSensor, sensorPotentiometer)
#pragma config(Sensor, dgtl1, EncoderSensor, sensorQuadEncoder)
#pragma config(Motor, port2, leftFrontBase, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port3, leftBackBase, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port4, rightFrontBase, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port5, rightBackBase, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port6, liftMotor1, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port7, liftMotor2, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port8, liftMotor3, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port9, liftMotor4, tmotorVex393_MC29, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
task main() //Autonomous Code for a drive with 4 motors and a lift with 4 motors
{
//Have base drive until a certain point (Encoder Sensor involved with this)
while(SensorValue[EncoderSensor] < /*Some value but I'll use whatever*/ 100)
{
motor[leftFrontBase] = /*Some value but I'll use whatever*/ 120;
motor[leftBackBase] = /*Some value but I'll use whatever*/ 120;
motor[rightFrontBase] = /*Some value but I'll use whatever*/ 120;
motor[rightBackBase] = /*Some value but I'll use whatever*/ 120;
//Have lift go up until a certain point (Potentiometer Sensor involved with this)
if(SensorValue[PotentiometerSensor] < /*Some value but I'll use whatever*/ 234)
{
motor[liftMotor1] = /*Some value but I'll use whatever*/ 120;
motor[liftMotor2] = /*Some value but I'll use whatever*/ 120;
motor[liftMotor3] = /*Some value but I'll use whatever*/ 120;
motor[liftMotor4] = /*Some value but I'll use whatever*/ 120;
}
else
{
motor[liftMotor1] = 0;
motor[liftMotor2] = 0;
motor[liftMotor3] = 0;
motor[liftMotor4] = 0;
}
}
}
This should work since the if statement will not keep looping. It will loop through the code if the condition is true. The else statement is there to make sure the motors wonât run after it has reached the desired height.
You can make multiple PID controllers run in series within a while loop. Then you just assign values to the PID controller while they run. This way your code loop is always running, and you are free to change the desired positions or motor values at will. I apologize for the super complex code, but check this out:
typedef struct{
int on;
float requestedValue;
int maxValue;
int minValue;
int stopMotion;
float sensorMult;
float kp;
float ki;
float kd;
float kbias;
float sensorVal;
float err;
float derv;
float integral;
float integralLimit;
float lasterr;
float drive;
} pidController;
//make a PID controller method
void PIDarm1(pidController pid1)
{
pid1.err= pid1.requestedValue - SensorValue(arm1encoder1);
if ( pid1.err<3 && pid1.err>-3) {pid1.err=0; pid1.integral=0}
pid1.derv = .err - pid1.lasterr;
pid1.lasterr = pid1.err;
pid1.integral += pid1.err;
//limit to avoid windup
if( pid1.integral > pid1.integralLimit) pid1.integral= pid1.integralLimit;
if( pid1.integral < -pid1.integralLimit) pid1.integral= -pid1.integralLimit;
pid1.drive = (pid1.kp*pid1.err)+(pid1.integral*pid1.ki)+(pid1.derv*pid1.kd)+pid1.kbias;
// drive should be in the range +/- 1.0
if( pid1.drive > 1.0 ) pid1.drive = 1.0;
if( pid1.drive < -1.0 ) pid1.drive = -1.0;
// Send to the motor
motor[motor1L]= pid1.drive*127;
motor[motor1R]= pid1.drive*127;
wait1Msec(0);
}
void PIDarm2(pidController pid2)
{
pid2.err= pid2.requestedValue - SensorValue(arm2encoder1);
if ( pid2.err<3 && pid2.err>-3) {pid2.err=0; pid2.integral=0}
pid2.sensorVal=SensorValue(arm2encoder1);
pid2.derv = pid2.err - pid2.lasterr;
pid2.lasterr = pid2.err;
pid2.integral += pid2.err;
//limit to avoid windup
if( pid2.integral > pid2.integralLimit) pid2.integral= pid2.integralLimit;
if( pid2.integral < -pid2.integralLimit) pid2.integral= -pid2.integralLimit;
pid2.drive = (pid2.kp*pid2.err)+(pid2.integral*pid2.ki)+(pid2.derv*pid2.kd)+pid2.kbias;
// drive should be in the range +/- 1.0
// if( pid2.drive > 1.0 ) pid2.drive = 1.0;
// if( pid2.drive < -1.0 ) pid2.drive = -1.0;
// Send to the motor
motor[motor2]= pid2.drive*127;
wait1Msec(0);
}
//Then implement into a main method
task main(){
//first assign a lot of global varliables
pidController pid1; pidController pid2;
pid1.kp=0.02; pid1.ki = 0.000; pid1.kd = 0.16; pid1.kbias = 0; //feel free to tune these with POT values on the fly.
pid2.kp=0.004; pid2.ki = 0.0000; pid2.kd = 0.011; pid2.kbias = 0;
pid1.sensorMult=1; pid2.sensorMult=1;
pid1.on=1; pid2.on=1;
pid2.integralLimit = 240; pid1.integralLimit = 240;
//Then give it some values.
if (vexRT[ch6]>0){ //trigger #1
pid1.requestedvalue=something;
}
if (vexRT[ch5]>0){ //trigger #2
pid2.requestedvalue=somethingelse;
}
PIDarm1(pid1); //put the pid structure into the pid method.
PIDarm2(pid2);
}
wait1Msec(25);
}
}
Let me know if I made an error. I will edit this later.
Wouldnât this encounter problems since you arenât using pointers? Since C is pass by value wouldnât the integral and derivative parts of the loop be useless since their values would be reset to 0 every time the struct is passed to the method? Also, what is the point of the second PID method when you could (using pointers) just pass the reference of the relevant struct to a single method?
I actually implemented something similar to this in my PROS code:
#include "main.h"
void setTop(int speed){
motorSet(chainLeft, speed);
motorSet(chainRight, speed);
}
void updateSensors(){
BL.sensor = analogRead(BLPot);
BR.sensor = analogRead(BRPot);
}
double PIDdo(PID *thing){
updateSensors();
thing->error = thing->target - thing->sensor;
if(abs(thing->error) < 200){
if(abs(thing->error < 8)){
//thing->integral = 0;
}
else{
thing->integral += thing->error;
}
}
else{
thing->integral = 0;
}
//may need integral = 0 stuff
thing->derivative = thing->error - thing->previous_error;
thing->previous_error = thing->error;
return (thing->Kp*thing->error + thing->Ki*thing->integral + thing->Kd*thing->derivative);
}
void mainLoop(){
motorSet(liftLeft, ((int) PIDdo(&BL)));
motorSet(liftRight, ((int) PIDdo(&BR)));
setTop((int) PIDdo(&T));
}
and here are my structs just for completion (values are defined later):
#include <main.h>
PID BL = {
.Kp = 0, .Ki = 0, .Kd = 0, .error = 0, .previous_error = 0, .integral = 0, .derivative = 0, .target = 0, .sensor = 0
};
PID BR = {
.Kp = 0, .Ki = 0, .Kd = 0, .error = 0, .previous_error = 0, .integral = 0, .derivative = 0, .target = 0, .sensor = 0
};
PID T = {
.Kp = 0, .Ki = 0, .Kd = 0, .error = 0, .previous_error = 0, .integral = 0, .derivative = 0, .target = 0, .sensor = 0
};
TBH, I have not done too much research into generalizing my method, and now it seems so obvious to create one general method and call it for each instance. Thanks for the idea!
I know this is just an example and only runs once, but for the OP, you need to make sure this sits in a loop with a wait for some period at the bottom of the loop like 20ms or whatever works well for you. Also, this wait is included in your âgainâ of the system so if you change it it will require a change in your co-efficients depending upon how much it is changed.
Also, in a generic from it may not be just PID, it may be PID+C. Not necessary in all cases, but certainly helpful in some.
Actually this is taken directly from my current robot code, and I just call taskRunLoop from my main method and set the delay there. But OP, if not using taskRunLoop, then a while(true) loop and a sufficient delay will be necessary as TriDragon described.
On a slightly unrelated note (still directed at the OP), the &s and ->s are vital to the function that I have described, so make sure you include them.
What exactly is PID+C and how does it compare to normal PID?
As Bfeher would say at this point:
Remember that adding the
static
type to a variable in a function will prevent its values from being reset every time the function returns and is run, ergo you can forgo the usage of pointers as much as you have.
The C is just a constant that may be added as an offset to the calculation based upon a-priori knowledge of what your output requires. For instance, the vex motors have hysteresis starting around 15 or 20. So you could have C = 20. This allows your system to get to itâs target more quickly.
The values will not necessarily be reset, space will be made for them in the stack frame and they may contain random values so donât make any assumptions about âautoâ variables and their starting content within a function.