mirror of
https://github.com/supleed2/ELEC50003-P1-CW.git
synced 2024-09-19 20:16:16 +00:00
Added I term to drive pid
and checking for correct json
This commit is contained in:
Mee2001
parent
5e95a8d860
commit
6f6320e920
|
|
@ -124,19 +124,20 @@ int distance_y = 0;
|
|||
|
||||
int dist_to_move_prev_fl = 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_fr = 0;
|
||||
|
||||
int dist_to_move_prev_sl = 0;
|
||||
int dist_to_move_prev_sr = 0;
|
||||
unsigned long time_pid_prev_sl = 0;
|
||||
unsigned long time_pid_prev_sr = 0;
|
||||
int angle_to_move_prev = 0;
|
||||
unsigned long time_pid_prev_ang = 0;
|
||||
|
||||
float kpdrive = 0.055;
|
||||
float kddrive = 4.700;
|
||||
float kpdrive = 0.059;
|
||||
float kddrive = 7.900;
|
||||
float kidrive = 0.0000009;
|
||||
|
||||
float kpheading = 0.055;
|
||||
float kdheading = 4.700;
|
||||
float kpheading = 0.037;
|
||||
float kdheading = 3.60;
|
||||
|
||||
volatile byte movementflag = 0;
|
||||
volatile int xydat[2];
|
||||
|
|
@ -152,8 +153,8 @@ void mousecam_write_reg(int reg, int val);
|
|||
int mousecam_read_reg(int reg);
|
||||
void mousecam_reset();
|
||||
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_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_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_rotation(int angle_to_move, int *angle_to_move_prev, unsigned long *time_pid_prev, float kps, float kds);
|
||||
|
||||
int convTwosComp(int b)
|
||||
{
|
||||
|
|
@ -293,7 +294,6 @@ int distTravelled_mm = 0;
|
|||
bool initialAngleSet = false;
|
||||
|
||||
//calibration varibles
|
||||
int angularDrift = 0; //+ve to right, -ve to left
|
||||
int leftStart = 80; //pwm min for left motor
|
||||
int leftStop = 255; //pwm max for left motor
|
||||
int rightStart = 80; //pwm min for right motor
|
||||
|
|
@ -423,11 +423,11 @@ DynamicJsonDocument rdoc(1024);
|
|||
|
||||
void loop()
|
||||
{
|
||||
if (Serial1.available() && (commandCompletionStatus == 0)){
|
||||
if (Serial1.available() && ((commandCompletionStatus == 0)||(commandCompletionStatus == 2))){
|
||||
// receive doc, not sure how big this needs to be
|
||||
error = deserializeJson(rdoc, Serial1);
|
||||
|
||||
Serial.println("Got serial");
|
||||
//Serial.println("Got serial");
|
||||
|
||||
// Test if parsing succeeds.
|
||||
if (error)
|
||||
|
|
@ -438,19 +438,25 @@ void loop()
|
|||
}
|
||||
else
|
||||
{
|
||||
if(rdoc.containsKey("sp") && rdoc["rH"] != -1){
|
||||
//parsing success, prepare command and pull request information
|
||||
commandCompletionStatus = 1;
|
||||
requiredHeading = rdoc["rH"];
|
||||
distance = rdoc["dist"];
|
||||
spd = rdoc["sp"];
|
||||
currentHeading = rdoc["cH"];
|
||||
currentHeading = int(rdoc["cH"]) + 180;
|
||||
|
||||
Serial.println("rH = " + String(requiredHeading) + " dist = " + String(distance) + " speed = " + String(spd));
|
||||
|
||||
//reset variables for update on completion
|
||||
commandComplete = 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
|
||||
if (currentHeading < requiredHeading)
|
||||
{ //turn right
|
||||
Serial.println("turning right");
|
||||
analogWrite(pwmr, getPWMfromSpeed(spd, false));
|
||||
analogWrite(pwml, getPWMfromSpeed(spd, true));
|
||||
//Serial.println("turning right");
|
||||
//Serial.println(currentHeading);
|
||||
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(DIRL, LOW);
|
||||
}
|
||||
else if (currentHeading > requiredHeading)
|
||||
{ //turn left
|
||||
Serial.println("turning left");
|
||||
analogWrite(pwmr, getPWMfromSpeed(spd, false));
|
||||
analogWrite(pwml, getPWMfromSpeed(spd, true));
|
||||
//Serial.println("turning left");
|
||||
//Serial.println(currentHeading);
|
||||
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(DIRL, HIGH);
|
||||
}
|
||||
|
|
@ -515,10 +525,8 @@ void loop()
|
|||
if (total_y - distance < 0)
|
||||
{ //go 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_l = pid_ms(abs(total_y - distance), &dist_to_move_prev_fl, &time_pid_prev_fl, kpdrive, kddrive);
|
||||
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);
|
||||
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, &dist_to_move_acc_fl, &time_pid_prev_fl, kpdrive, kddrive, kidrive);
|
||||
analogWrite(pwmr, getPWMfromSpeed(speed_r, false));
|
||||
analogWrite(pwml, getPWMfromSpeed(speed_l, true));
|
||||
digitalWrite(DIRR, LOW);
|
||||
|
|
@ -527,10 +535,8 @@ void loop()
|
|||
else if (total_y - distance > 0)
|
||||
{ //go 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_l = pid_ms(abs(total_y - distance), &dist_to_move_prev_fl, &time_pid_prev_fl, kpdrive, kddrive);
|
||||
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);
|
||||
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, &dist_to_move_acc_fl, &time_pid_prev_fl, kpdrive, kddrive, kidrive);
|
||||
analogWrite(pwmr, getPWMfromSpeed(speed_r, false));
|
||||
analogWrite(pwml, getPWMfromSpeed(speed_l, true));
|
||||
digitalWrite(DIRR, HIGH);
|
||||
|
|
@ -547,7 +553,7 @@ void loop()
|
|||
}
|
||||
}
|
||||
if (commandCompletionStatus == 3)
|
||||
{ Serial.println("status3");
|
||||
{ // Serial.println("status3");
|
||||
//currentPosMatchesOrExceedsRequest
|
||||
///finish moving
|
||||
|
||||
|
|
@ -585,6 +591,7 @@ void loop()
|
|||
tdoc["mm"] = distTravelled_mm;
|
||||
tdoc["pos"][0] = current_x;
|
||||
tdoc["pos"][1] = current_y;
|
||||
serializeJson(tdoc, Serial1); // Build JSON and send on UART1
|
||||
serializeJson(tdoc, Serial); // Build JSON and send on UART1
|
||||
commandCompletionStatus = 0;
|
||||
}
|
||||
|
|
@ -635,7 +642,7 @@ void loop()
|
|||
Serial.println(')'); */
|
||||
|
||||
// Serial.println(md.max_pix);
|
||||
delay(100);
|
||||
delay(50);
|
||||
|
||||
distance_x = md.dx; //convTwosComp(md.dx);
|
||||
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.
|
||||
// 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
|
||||
|
||||
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;
|
||||
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();
|
||||
|
||||
Serial.println(speed);
|
||||
|
||||
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;
|
||||
return speed;
|
||||
}
|
||||
|
||||
// This is a P!ID contrller for heading using optical flow
|
||||
|
||||
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 T_diff = millis() - *time_pid_prev;
|
||||
float speed_aug;
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
float speed = (kps * angle_to_move) + ((kds/T_diff) * (angle_to_move - *angle_to_move_prev));
|
||||
*time_pid_prev = millis();
|
||||
|
||||
if (speed_aug >= 1) speed_aug = 1;
|
||||
else if (speed_aug <= 0.3) speed_aug = 0.3;
|
||||
Serial.println(speed);
|
||||
|
||||
*dist_to_move_prev = dist_to_move;
|
||||
return speed_aug;
|
||||
if (speed >= 1) speed = 1;
|
||||
else if (speed <= 0.85) speed = 0.85;
|
||||
|
||||
*angle_to_move_prev = angle_to_move;
|
||||
return speed;
|
||||
}
|
||||
|
|
@ -3,6 +3,9 @@
|
|||
#include <ArduinoJson.h>
|
||||
|
||||
float pitch, roll, yaw;
|
||||
// unsigned long previous_millis;
|
||||
// unsigned long current_millis;
|
||||
double yaw_correction = 0;
|
||||
|
||||
void setup() {
|
||||
M5.begin();
|
||||
|
|
@ -13,13 +16,23 @@ void setup() {
|
|||
void loop() {
|
||||
M5.IMU.getAhrsData(&pitch, &roll, &yaw);
|
||||
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);
|
||||
// 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);
|
||||
tdoc["cH"] = roll;
|
||||
tdoc["cH"] = yaw;
|
||||
serializeJson(tdoc, Serial1);
|
||||
|
||||
|
||||
delay(50);
|
||||
|
||||
delay(52);
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in a new issue