supleed2/ELEC50003-P1-CW
1
0
Fork 0
mirror of https://github.com/supleed2/ELEC50003-P1-CW.git synced 2024-12-22 21:45:49 +00:00
Code Issues Packages Projects Releases Wiki Activity

Implemented PID control

Browse source 
for forward and backward motion
This commit is contained in:
Mee2001 2021-06-10 17:34:22 +01:00
parent f40452c255
commit bb33d95697
1 changed files with 30 additions and 4 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

34
Drive/src/main.cpp
View file

@ -122,6 +122,14 @@ int b = 0;
int distance_x = 0; int distance_x = 0;
int distance_y = 0; int distance_y = 0;
int dist_to_move_prev_fl = 0;
int dist_to_move_prev_fr = 0;
unsigned long time_pid_prev_fl = 0;
unsigned long time_pid_prev_fr = 0;
float kpdrive = 0.055;
float kddrive = 4.700;
volatile byte movementflag = 0; volatile byte movementflag = 0;
volatile int xydat[2]; volatile int xydat[2];
@ -136,6 +144,7 @@ void mousecam_write_reg(int reg, int val);
int mousecam_read_reg(int reg); int mousecam_read_reg(int reg);
void mousecam_reset(); void mousecam_reset();
int getCurrentHeading(); int getCurrentHeading();
float pid_ms(int dist_to_move, int *dist_to_move_prev, unsigned long *time_pid_prev, float kps, float kds);
int convTwosComp(int b) int convTwosComp(int b)
{ {
@ -498,16 +507,16 @@ void loop()
if (total_x - distance < 0) if (total_x - distance < 0)
{ //go forwards { //go forwards
Serial.println("going forwards"); Serial.println("going forwards");
analogWrite(pwmr, getPWMfromSpeed(spd, false)); analogWrite(pwmr, getPWMfromSpeed(pid_ms(abs(total_x - distance), &dist_to_move_prev_fr, &time_pid_prev_fr, kpdrive, kddrive), false));
analogWrite(pwml, getPWMfromSpeed(spd, true)); analogWrite(pwml, getPWMfromSpeed(pid_ms(abs(total_x - distance), &dist_to_move_prev_fl, &time_pid_prev_fl, kpdrive, kddrive), true));
digitalWrite(DIRR, LOW); digitalWrite(DIRR, LOW);
digitalWrite(DIRL, HIGH); digitalWrite(DIRL, HIGH);
} }
else if (total_x - distance > 0) else if (total_x - distance > 0)
{ //go backwards { //go backwards
Serial.println("going backwards"); Serial.println("going backwards");
analogWrite(pwmr, getPWMfromSpeed(spd, false)); analogWrite(pwmr, getPWMfromSpeed(pid_ms(abs(total_x - distance), &dist_to_move_prev_fr, &time_pid_prev_fr, kpdrive, kddrive), false));
analogWrite(pwml, getPWMfromSpeed(spd, true)); analogWrite(pwml, getPWMfromSpeed(pid_ms(abs(total_x - distance), &dist_to_move_prev_fl, &time_pid_prev_fl, kpdrive, kddrive), true));
digitalWrite(DIRR, HIGH); digitalWrite(DIRR, HIGH);
digitalWrite(DIRL, LOW); digitalWrite(DIRL, LOW);
} }
@ -802,4 +811,21 @@ float pidi(float pid_input){
e2i = e1i; //update last last time's error e2i = e1i; //update last last time's error
e1i = e0i; // update last time's error e1i = e0i; // update last time's error
return u0i; return u0i;
}
// This is a P!ID contrller for motor speed
float pid_ms(int dist_to_move, int *dist_to_move_prev, unsigned long *time_pid_prev, float kps, float kds){
int T_diff = millis() - *time_pid_prev;
float speed = (kps * dist_to_move) + ((kds/T_diff) * (dist_to_move - *dist_to_move_prev));
*time_pid_prev = millis();
Serial.println(speed);
if (speed >= 1) speed = 1;
else if (speed <= 0.5) speed = 0.5;
*dist_to_move_prev = dist_to_move;
return speed;
} }