supleed2/ELEC50003-P1-CW
1
0
Fork 0
mirror of https://github.com/supleed2/ELEC50003-P1-CW.git synced 2024-09-19 20:16:16 +00:00
Code Issues Packages Projects Releases Wiki Activity

Added I term to drive pid

Browse source 
and checking for correct json
This commit is contained in:
Mee2001 2021-06-15 03:38:44 +01:00
parent 5e95a8d860
commit 6f6320e920
2 changed files with 76 additions and 71 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

126
Drive/src/main.cpp
View file

@ -124,19 +124,20 @@ int distance_y = 0;
int dist_to_move_prev_fl = 0; int dist_to_move_prev_fl = 0;
int dist_to_move_prev_fr = 0; int dist_to_move_prev_fr = 0;
int dist_to_move_acc_fl = 0;
int dist_to_move_acc_fr = 0;
unsigned long time_pid_prev_fl = 0; unsigned long time_pid_prev_fl = 0;
unsigned long time_pid_prev_fr = 0; unsigned long time_pid_prev_fr = 0;
int dist_to_move_prev_sl = 0; int angle_to_move_prev = 0;
int dist_to_move_prev_sr = 0; unsigned long time_pid_prev_ang = 0;
unsigned long time_pid_prev_sl = 0;
unsigned long time_pid_prev_sr = 0;
float kpdrive = 0.055; float kpdrive = 0.059;
float kddrive = 4.700; float kddrive = 7.900;
float kidrive = 0.0000009;
float kpheading = 0.055; float kpheading = 0.037;
float kdheading = 4.700; float kdheading = 3.60;
volatile byte movementflag = 0; volatile byte movementflag = 0;
volatile int xydat[2]; volatile int xydat[2];
@ -152,8 +153,8 @@ 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); float pid_ms(int dist_to_move, int *dist_to_move_prev, int *dist_to_move_acc, unsigned long *time_pid_prev, float kps, float kds, float kis);
float pid_h_ms(bool left, float speed, int dist_to_move, int *dist_to_move_prev, unsigned long *time_pid_prev, float kps, float kds); float pid_rotation(int angle_to_move, int *angle_to_move_prev, unsigned long *time_pid_prev, float kps, float kds);
int convTwosComp(int b) int convTwosComp(int b)
{ {
@ -293,7 +294,6 @@ int distTravelled_mm = 0;
bool initialAngleSet = false; bool initialAngleSet = false;
//calibration varibles //calibration varibles
int angularDrift = 0; //+ve to right, -ve to left
int leftStart = 80; //pwm min for left motor int leftStart = 80; //pwm min for left motor
int leftStop = 255; //pwm max for left motor int leftStop = 255; //pwm max for left motor
int rightStart = 80; //pwm min for right motor int rightStart = 80; //pwm min for right motor
@ -423,11 +423,11 @@ DynamicJsonDocument rdoc(1024);
void loop() void loop()
{ {
if (Serial1.available() && (commandCompletionStatus == 0)){ if (Serial1.available() && ((commandCompletionStatus == 0)||(commandCompletionStatus == 2))){
// receive doc, not sure how big this needs to be // receive doc, not sure how big this needs to be
error = deserializeJson(rdoc, Serial1); error = deserializeJson(rdoc, Serial1);
Serial.println("Got serial"); //Serial.println("Got serial");
// Test if parsing succeeds. // Test if parsing succeeds.
if (error) if (error)
@ -438,19 +438,25 @@ void loop()
} }
else else
{ {
//parsing success, prepare command and pull request information if(rdoc.containsKey("sp") && rdoc["rH"] != -1){
commandCompletionStatus = 1; //parsing success, prepare command and pull request information
requiredHeading = rdoc["rH"]; commandCompletionStatus = 1;
distance = rdoc["dist"]; requiredHeading = rdoc["rH"];
spd = rdoc["sp"]; distance = rdoc["dist"];
currentHeading = rdoc["cH"]; spd = rdoc["sp"];
currentHeading = int(rdoc["cH"]) + 180;
Serial.println("rH = " + String(requiredHeading) + " dist = " + String(distance) + " speed = " + String(spd)); Serial.println("rH = " + String(requiredHeading) + " dist = " + String(distance) + " speed = " + String(spd));
//reset variables for update on completion //reset variables for update on completion
commandComplete = 0; commandComplete = 0;
powerUsage_mWh = 0.0; powerUsage_mWh = 0.0;
distTravelled_mm = 0; dist_to_move_acc_fl = 0;
dist_to_move_acc_fr = 0;
}else if (rdoc.containsKey("cH")){
currentHeading = 180 + int(rdoc["cH"]);
// Serial.println(currentHeading);
}
} }
} }
@ -486,17 +492,21 @@ void loop()
//turn to angle //turn to angle
if (currentHeading < requiredHeading) if (currentHeading < requiredHeading)
{ //turn right { //turn right
Serial.println("turning right"); //Serial.println("turning right");
analogWrite(pwmr, getPWMfromSpeed(spd, false)); //Serial.println(currentHeading);
analogWrite(pwml, getPWMfromSpeed(spd, true)); float spd_pid = pid_rotation(abs(currentHeading - requiredHeading), &angle_to_move_prev, &time_pid_prev_ang, kpheading, kdheading);
analogWrite(pwmr, getPWMfromSpeed(spd_pid, false));
analogWrite(pwml, getPWMfromSpeed(spd_pid, true));
digitalWrite(DIRR, LOW); digitalWrite(DIRR, LOW);
digitalWrite(DIRL, LOW); digitalWrite(DIRL, LOW);
} }
else if (currentHeading > requiredHeading) else if (currentHeading > requiredHeading)
{ //turn left { //turn left
Serial.println("turning left"); //Serial.println("turning left");
analogWrite(pwmr, getPWMfromSpeed(spd, false)); //Serial.println(currentHeading);
analogWrite(pwml, getPWMfromSpeed(spd, true)); float spd_pid = pid_rotation(abs(currentHeading - requiredHeading), &angle_to_move_prev, &time_pid_prev_ang, kpheading, kdheading);
analogWrite(pwmr, getPWMfromSpeed(spd_pid, false));
analogWrite(pwml, getPWMfromSpeed(spd_pid, true));
digitalWrite(DIRR, HIGH); digitalWrite(DIRR, HIGH);
digitalWrite(DIRL, HIGH); digitalWrite(DIRL, HIGH);
} }
@ -515,10 +525,8 @@ void loop()
if (total_y - distance < 0) if (total_y - distance < 0)
{ //go forwards { //go forwards
//Serial.println("going forwards"); //Serial.println("going forwards");
float speed_r = pid_ms(abs(total_y - distance), &dist_to_move_prev_fr, &time_pid_prev_fr, kpdrive, kddrive); float speed_r = pid_ms(abs(total_y - distance), &dist_to_move_prev_fr, &dist_to_move_acc_fr, &time_pid_prev_fr, kpdrive, kddrive, kidrive);
float speed_l = pid_ms(abs(total_y - distance), &dist_to_move_prev_fl, &time_pid_prev_fl, kpdrive, kddrive); float speed_l = pid_ms(abs(total_y - distance), &dist_to_move_prev_fl, &dist_to_move_acc_fl, &time_pid_prev_fl, kpdrive, kddrive, kidrive);
float speed_r_head = pid_h_ms(0, speed_r, -total_x, &dist_to_move_prev_sr, &time_pid_prev_sr, kpheading, kdheading);
float speed_l_head = pid_h_ms(1, speed_l, -total_x, &dist_to_move_prev_sl, &time_pid_prev_sl, kpheading, kdheading);
analogWrite(pwmr, getPWMfromSpeed(speed_r, false)); analogWrite(pwmr, getPWMfromSpeed(speed_r, false));
analogWrite(pwml, getPWMfromSpeed(speed_l, true)); analogWrite(pwml, getPWMfromSpeed(speed_l, true));
digitalWrite(DIRR, LOW); digitalWrite(DIRR, LOW);
@ -527,10 +535,8 @@ void loop()
else if (total_y - distance > 0) else if (total_y - distance > 0)
{ //go backwards { //go backwards
//Serial.println("going backwards"); //Serial.println("going backwards");
float speed_r = pid_ms(abs(total_y - distance), &dist_to_move_prev_fr, &time_pid_prev_fr, kpdrive, kddrive); float speed_r = pid_ms(abs(total_y - distance), &dist_to_move_prev_fr, &dist_to_move_acc_fr, &time_pid_prev_fr, kpdrive, kddrive, kidrive);
float speed_l = pid_ms(abs(total_y - distance), &dist_to_move_prev_fl, &time_pid_prev_fl, kpdrive, kddrive); float speed_l = pid_ms(abs(total_y - distance), &dist_to_move_prev_fl, &dist_to_move_acc_fl, &time_pid_prev_fl, kpdrive, kddrive, kidrive);
float speed_r_head = pid_h_ms(0, speed_r, -total_x, &dist_to_move_prev_sr, &time_pid_prev_sr, kpheading, kdheading);
float speed_l_head = pid_h_ms(1, speed_l, -total_x, &dist_to_move_prev_sl, &time_pid_prev_sl, kpheading, kdheading);
analogWrite(pwmr, getPWMfromSpeed(speed_r, false)); analogWrite(pwmr, getPWMfromSpeed(speed_r, false));
analogWrite(pwml, getPWMfromSpeed(speed_l, true)); analogWrite(pwml, getPWMfromSpeed(speed_l, true));
digitalWrite(DIRR, HIGH); digitalWrite(DIRR, HIGH);
@ -547,7 +553,7 @@ void loop()
} }
} }
if (commandCompletionStatus == 3) if (commandCompletionStatus == 3)
{ Serial.println("status3"); { // Serial.println("status3");
//currentPosMatchesOrExceedsRequest //currentPosMatchesOrExceedsRequest
///finish moving ///finish moving
@ -585,6 +591,7 @@ void loop()
tdoc["mm"] = distTravelled_mm; tdoc["mm"] = distTravelled_mm;
tdoc["pos"][0] = current_x; tdoc["pos"][0] = current_x;
tdoc["pos"][1] = current_y; tdoc["pos"][1] = current_y;
serializeJson(tdoc, Serial1); // Build JSON and send on UART1
serializeJson(tdoc, Serial); // Build JSON and send on UART1 serializeJson(tdoc, Serial); // Build JSON and send on UART1
commandCompletionStatus = 0; commandCompletionStatus = 0;
} }
@ -635,7 +642,7 @@ void loop()
Serial.println(')'); */ Serial.println(')'); */
// Serial.println(md.max_pix); // Serial.println(md.max_pix);
delay(100); delay(50);
distance_x = md.dx; //convTwosComp(md.dx); distance_x = md.dx; //convTwosComp(md.dx);
distance_y = md.dy; //convTwosComp(md.dy); distance_y = md.dy; //convTwosComp(md.dy);
@ -715,11 +722,6 @@ void loop()
} }
} }
int getCurrentHeading(){
int currentAngle = 0; //-------------___<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
return currentAngle;
}
// Timer A CMP1 interrupt. Every 800us the program enters this interrupt. // Timer A CMP1 interrupt. Every 800us the program enters this interrupt.
// This, clears the incoming interrupt flag and triggers the main loop. // This, clears the incoming interrupt flag and triggers the main loop.
@ -831,42 +833,32 @@ float pidi(float pid_input){
// This is a P!ID contrller for motor speed // 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){ float pid_ms(int dist_to_move, int *dist_to_move_prev, int *dist_to_move_acc, unsigned long *time_pid_prev, float kps, float kds, float kis){
int T_diff = millis() - *time_pid_prev; int T_diff = millis() - *time_pid_prev;
float speed = (kps * dist_to_move) + ((kds/T_diff) * (dist_to_move - *dist_to_move_prev)); *dist_to_move_acc = *dist_to_move_acc + dist_to_move;
float speed = (kps * dist_to_move) + ((kds/T_diff) * (dist_to_move - *dist_to_move_prev)) + ((kis*T_diff) * (*dist_to_move_acc));
*time_pid_prev = millis(); *time_pid_prev = millis();
Serial.println(speed); Serial.println(speed);
if (speed >= 1) speed = 1; if (speed >= 1) speed = 1;
else if (speed <= 0.5) speed = 0.5; else if (speed <= 0.55) speed = 0.55;
*dist_to_move_prev = dist_to_move; *dist_to_move_prev = dist_to_move;
return speed; return speed;
} }
// This is a P!ID contrller for heading using optical flow float pid_rotation(int angle_to_move, int *angle_to_move_prev, unsigned long *time_pid_prev, float kps, float kds){
float pid_h_ms(bool left, float speed, 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; int T_diff = millis() - *time_pid_prev;
float speed_aug; float speed = (kps * angle_to_move) + ((kds/T_diff) * (angle_to_move - *angle_to_move_prev));
if ((dist_to_move > 0 && !left )||( dist_to_move < 0 && left)){
float speed_aug_mult = (kps * dist_to_move) + ((kds/T_diff) * (dist_to_move - *dist_to_move_prev));
if (speed_aug_mult >= 2) speed_aug_mult = 2;
else if (speed_aug_mult <= 1) speed_aug_mult = 1;
speed_aug = speed / dist_to_move;
}else{
speed_aug = speed;
}
*time_pid_prev = millis(); *time_pid_prev = millis();
if (speed_aug >= 1) speed_aug = 1; Serial.println(speed);
else if (speed_aug <= 0.3) speed_aug = 0.3;
*dist_to_move_prev = dist_to_move; if (speed >= 1) speed = 1;
return speed_aug; else if (speed <= 0.85) speed = 0.85;
*angle_to_move_prev = angle_to_move;
return speed;
} }

21
IMU/src/main.cpp
View file

@ -3,6 +3,9 @@
#include <ArduinoJson.h> #include <ArduinoJson.h>
float pitch, roll, yaw; float pitch, roll, yaw;
// unsigned long previous_millis;
// unsigned long current_millis;
double yaw_correction = 0;
void setup() { void setup() {
M5.begin(); M5.begin();
@ -13,13 +16,23 @@ void setup() {
void loop() { void loop() {
M5.IMU.getAhrsData(&pitch, &roll, &yaw); M5.IMU.getAhrsData(&pitch, &roll, &yaw);
M5.Lcd.setCursor(0, 45); M5.Lcd.setCursor(0, 45);
M5.Lcd.printf("%5.2f\n", roll); M5.Lcd.printf("%5.2f\n", yaw);
// Serial.printf("{\"cH\":%5.2f}\n", roll); // Serial.printf("{\"cH\":%5.2f}\n", roll);
// Serial1.printf("{\"cH\":%5.2f}\n", roll); // Serial1.printf("{\"cH\":%5.2f}\n", roll);
// current_millis = millis();
// float t_diff = (current_millis - previous_millis);
// previous_millis = current_millis;
yaw_correction = yaw_correction + (0.041808);
Serial.println(yaw + yaw_correction);
DynamicJsonDocument tdoc(1024); DynamicJsonDocument tdoc(1024);
tdoc["cH"] = roll; tdoc["cH"] = yaw;
serializeJson(tdoc, Serial1); serializeJson(tdoc, Serial1);
delay(50);
} delay(52);
}