diff --git a/Control/data/command.js b/Control/data/command.js
index 091ecc2..e17fb44 100644
--- a/Control/data/command.js
+++ b/Control/data/command.js
@@ -75,9 +75,9 @@ var command = (function(){
var o=function(k,v,o,l){for(o=o||{},l=k.length;l--;o[k[l]]=v);return o};
var parser = {trace: function trace () { },
yy: {},
-symbols_: {"error":2,"command":3,"expr":4,"EOF":5,"move":6,"whitespace":7,"distance":8,"heading_angle":9,"percentage":10,"stop":11,"pstop":12,"stop_duration":13,"charge_to":14,"telemetry_reset":15,"help":16,"$accept":0,"$end":1},
-terminals_: {2:"error",5:"EOF",6:"move",7:"whitespace",8:"distance",9:"heading_angle",10:"percentage",11:"stop",12:"pstop",13:"stop_duration",14:"charge_to",15:"telemetry_reset",16:"help"},
-productions_: [0,[3,2],[4,7],[4,1],[4,3],[4,3],[4,1],[4,1]],
+symbols_: {"error":2,"command":3,"expr":4,"EOF":5,"move":6,"whitespace":7,"distance":8,"heading_angle":9,"percentage":10,"stop":11,"pstop":12,"stop_duration":13,"charge_to":14,"colour":15,"colour_name":16,"telemetry_reset":17,"help":18,"$accept":0,"$end":1},
+terminals_: {2:"error",5:"EOF",6:"move",7:"whitespace",8:"distance",9:"heading_angle",10:"percentage",11:"stop",12:"pstop",13:"stop_duration",14:"charge_to",15:"colour",16:"colour_name",17:"telemetry_reset",18:"help"},
+productions_: [0,[3,2],[4,7],[4,1],[4,3],[4,3],[4,3],[4,1],[4,1]],
performAction: function anonymous(yytext, yyleng, yylineno, yy, yystate /* action[1] */, $$ /* vstack */, _$ /* lstack */) {
/* this == yyval */
@@ -109,19 +109,25 @@ case 5:
break;
case 6:
+
+ var inColour = String($$[$0]);
+ colourCmd(inColour);
+
+break;
+case 7:
telRst();
break;
-case 7:
+case 8:
printHelpDetails();
break;
}
},
-table: [{3:1,4:2,6:[1,3],11:[1,4],12:[1,5],14:[1,6],15:[1,7],16:[1,8]},{1:[3]},{5:[1,9]},{7:[1,10]},{5:[2,3]},{7:[1,11]},{7:[1,12]},{5:[2,6]},{5:[2,7]},{1:[2,1]},{8:[1,13]},{13:[1,14]},{10:[1,15]},{7:[1,16]},{5:[2,4]},{5:[2,5]},{9:[1,17]},{7:[1,18]},{10:[1,19]},{5:[2,2]}],
-defaultActions: {4:[2,3],7:[2,6],8:[2,7],9:[2,1],14:[2,4],15:[2,5],19:[2,2]},
+table: [{3:1,4:2,6:[1,3],11:[1,4],12:[1,5],14:[1,6],15:[1,7],17:[1,8],18:[1,9]},{1:[3]},{5:[1,10]},{7:[1,11]},{5:[2,3]},{7:[1,12]},{7:[1,13]},{7:[1,14]},{5:[2,7]},{5:[2,8]},{1:[2,1]},{8:[1,15]},{13:[1,16]},{10:[1,17]},{16:[1,18]},{7:[1,19]},{5:[2,4]},{5:[2,5]},{5:[2,6]},{9:[1,20]},{7:[1,21]},{10:[1,22]},{5:[2,2]}],
+defaultActions: {4:[2,3],8:[2,7],9:[2,8],10:[2,1],16:[2,4],17:[2,5],18:[2,6],22:[2,2]},
parseError: function parseError (str, hash) {
if (hash.recoverable) {
this.trace(str);
@@ -612,20 +618,24 @@ case 6:return 12
break;
case 7:return 11
break;
-case 8:return 16
+case 8:return 18
break;
case 9:return 14
break;
-case 10:return 15
+case 10:return 17
break;
-case 11:return 5
+case 11:return 15
break;
-case 12:return 'invalid_command'
+case 12:return 16
+break;
+case 13:return 5
+break;
+case 14:return 'invalid_command'
break;
}
},
-rules: [/^(?:\s)/,/^(?:\b[0-9]+mm\b)/,/^(?:\b([0-9]|[1-8][0-9]|9[0-9]|[12][0-9]{2}|3[0-4][0-9]|35[0-9])deg\b)/,/^(?:\b([0-9]|[1-8][0-9]|9[0-9]|100)%)/,/^(?:\b[0-9]+s\b)/,/^(?:\bmove\b)/,/^(?:\bpstop\b)/,/^(?:\bstop\b)/,/^(?:\bhelp\b)/,/^(?:\bcharge\sto\b)/,/^(?:\btelemetry\sreset\b)/,/^(?:$)/,/^(?:.)/],
-conditions: {"INITIAL":{"rules":[0,1,2,3,4,5,6,7,8,9,10,11,12],"inclusive":true}}
+rules: [/^(?:\s)/,/^(?:(-[0-9]+mm|[0-9]+mm))/,/^(?:([0-9]|[1-8][0-9]|9[0-9]|[12][0-9]{2}|3[0-4][0-9]|35[0-9])deg\b)/,/^(?:([0-9]|[1-8][0-9]|9[0-9]|100)%)/,/^(?:[0-9]+s\b)/,/^(?:move\b)/,/^(?:pstop\b)/,/^(?:stop\b)/,/^(?:help\b)/,/^(?:charge\sto\b)/,/^(?:telemetry\sreset\b)/,/^(?:colour\b)/,/^(?:(red|blue|green|pink|orange))/,/^(?:$)/,/^(?:.)/],
+conditions: {"INITIAL":{"rules":[0,1,2,3,4,5,6,7,8,9,10,11,12,13,14],"inclusive":true}}
});
return lexer;
})();
diff --git a/Control/data/index.html b/Control/data/index.html
index 12f4278..2e9db40 100644
--- a/Control/data/index.html
+++ b/Control/data/index.html
@@ -130,28 +130,28 @@
- |
- |
- |
+ |
+ |
+ |
|
Set Heading to: 270° |
- |
+ |
Set Translation to: 180mm |
- |
+ |
Set Speed to: 50% |
- |
+ |
|
@@ -235,6 +235,7 @@
var reqSpeed = 0;
var reqCharge = 0;
var pstop_time = 0;
+ var hunt_colour = "";
var state = 0;
var batteryVoltage = 0;
@@ -255,11 +256,11 @@
var data = JSON.parse(raw_data);
state = data.st;
- batteryLevel = data.bV;
+ batteryLevel = data.bL;
batteryVoltage = data.bV;
batteryCycles = data.bC;
totalTripDistance = data.tD;
- currentHeading = data.cH;
+ currentHeading = 180 - data.cH;
current_pos = data.pos;
current_x = current_pos[0];
current_y = current_pos[1];
@@ -275,6 +276,8 @@
rStatus = "Moving";
}else if(state == 2){
rStatus = "Charging";
+ }else if(state == 3){
+ rStatus = "Waiting";
}else{
rStatus = "Undefined";
}
@@ -291,7 +294,7 @@
}
function send_data() {
- var raw_data = '{"Cid":' + command_id + ',"mode":' + mode + ',"rH":' + reqHeading + ',"rD":' + reqDistance + ',"rS":' + reqSpeed + ',"rC":' + reqCharge + ',"pSt":' + pstop_time + '}';
+ var raw_data = '{"Cid":' + command_id + ',"mode":' + mode + ',"rH":' + reqHeading + ',"rD":' + reqDistance + ',"rS":' + reqSpeed + ',"rC":' + reqCharge + ',"pSt":' + pstop_time + ',"col":"' + hunt_colour + '"}';
connection.send(raw_data);
console.log(raw_data);
}
@@ -306,8 +309,16 @@
bufferOutput = document.getElementById("commandInput").value;
}else if(cmdMode == 1){
bufferOutput = "move " + reqDistance + "mm " + reqHeading + "deg " + reqSpeed + "%";
+ }else if(cmdMode == 2){
+ bufferOutput = "charge " + reqCharge + "%";
+ }else if(cmdMode == 3){
+ bufferOutput = "wait " + pstop_time + "s";
+ }else if(cmdMode == 4){
+ bufferOutput = "colour " + hunt_colour;
+ }else if(cmdMode == -1){
+ bufferOutput = "telemetry reset";
}
- if((mode == 1) || (mode == -1) || (mode == 2) || (mode == 3)){
+ if((mode == 1) || (mode == -1) || (mode == 2) || (mode == 3) || (mode == 4)){
document.getElementById("command_buffer").innerHTML += '[' + command_id + '] ' + bufferOutput + "
";
document.getElementById("commandInput").value = ""
}else if(mode == 0){
@@ -338,11 +349,12 @@
function moveCmd(dist,hdg,spd){
mode = 1;
reqDistance = dist;
- reqHeading = hdg;
+ reqHeading = 360 - hdg;
reqSpeed = spd;
reqCharge = 0;
pstop_time = 0;
tel_rst = 0;
+ hunt_colour = "";
send_data();
updateCommandBuffer();
command_id++;
@@ -356,6 +368,7 @@
reqCharge = 0;
pstop_time = 0;
tel_rst = 0;
+ hunt_colour = "";
send_data();
command_id = 1;
updateCommandBuffer();
@@ -369,6 +382,7 @@
reqCharge = 0;
pstop_time = pstp_tme;
tel_rst = 0;
+ hunt_colour = "";
send_data();
updateCommandBuffer();
command_id++;
@@ -382,6 +396,7 @@
reqCharge = chrglvl;
pstop_time = 0;
tel_rst = 0;
+ hunt_colour = "";
send_data();
updateCommandBuffer();
command_id++;
@@ -394,6 +409,20 @@
reqSpeed = 0;
reqCharge = 0;
pstop_time = 0;
+ hunt_colour = "";
+ send_data();
+ updateCommandBuffer();
+ command_id++;
+ }
+
+ function colourCmd(clrName){
+ mode = 4;
+ reqDistance = 0;
+ reqHeading = 0;
+ reqSpeed = 0;
+ reqCharge = 0;
+ pstop_time = 0;
+ hunt_colour = clrName;
send_data();
updateCommandBuffer();
command_id++;
@@ -409,7 +438,7 @@
}
}
- var setHdgto = 270;
+ var setHdgto = 180;
var hdg_slider = document.getElementById("HdgSlider");
var hdg_output = document.getElementById("SetHeading");
hdg_output.innerHTML = hdg_slider.value;
@@ -418,7 +447,7 @@
setHdgto = this.value;
}
- var setTransto = 100;
+ var setTransto = 0;
var tran_slider = document.getElementById("TranSlider");
var tran_output = document.getElementById("SetTrans");
tran_output.innerHTML = tran_slider.value;
@@ -427,7 +456,7 @@
setTransto = this.value;
}
- var setSpdto = 50;
+ var setSpdto = 100;
var spd_slider = document.getElementById("SpdSlider");
var spd_output = document.getElementById("SetSpd");
spd_output.innerHTML = spd_slider.value;
diff --git a/Control/include/instruction.h b/Control/include/instruction.h
index dc110bb..d319cdd 100644
--- a/Control/include/instruction.h
+++ b/Control/include/instruction.h
@@ -1,5 +1,6 @@
#ifndef INSTRUCTION_H
#define INSTRUCTION_H
+#include
typedef enum {
INSTR_RESET = -1,
@@ -19,7 +20,7 @@ typedef struct instruction
float speed;
int charge;
int time;
- int colour;
+ String colour;
} RoverInstruction;
#endif
diff --git a/Control/src/command.jison b/Control/src/command.jison
index f36dfd9..7a0a911 100644
--- a/Control/src/command.jison
+++ b/Control/src/command.jison
@@ -1,19 +1,21 @@
%lex
%%
-\s return 'whitespace'
-\b[0-9]+"mm"\b return 'distance'
-\b([0-9]|[1-8][0-9]|9[0-9]|[12][0-9]{2}|3[0-4][0-9]|35[0-9])"deg"\b return 'heading_angle'
-\b([0-9]|[1-8][0-9]|9[0-9]|100)"%" return 'percentage'
-\b[0-9]+"s"\b return 'stop_duration'
-\bmove\b return 'move'
-\bpstop\b return 'pstop'
-\bstop\b return 'stop'
-\bhelp\b return 'help'
-\bcharge\sto\b return 'charge_to'
-\btelemetry\sreset\b return 'telemetry_reset'
-<> return 'EOF'
-. return 'invalid_command'
+\s return 'whitespace'
+("-"[0-9]+"mm"|[0-9]+"mm") return 'distance'
+([0-9]|[1-8][0-9]|9[0-9]|[12][0-9]{2}|3[0-4][0-9]|35[0-9])"deg" return 'heading_angle'
+([0-9]|[1-8][0-9]|9[0-9]|100)"%" return 'percentage'
+[0-9]+"s" return 'stop_duration'
+move return 'move'
+pstop return 'pstop'
+stop return 'stop'
+help return 'help'
+charge\sto return 'charge_to'
+telemetry\sreset return 'telemetry_reset'
+colour return 'colour'
+("red"|"blue"|"green"|"pink"|"orange") return 'colour_name'
+<> return 'EOF'
+. return 'invalid_command'
/lex
@@ -47,6 +49,11 @@ expr
var inChrg = Number(String($3).substr(0, ((String($3).length) - 1)));
chrgCmd(inChrg);
}
+ | colour whitespace colour_name
+ {
+ var inColour = String($3);
+ colourCmd(inColour);
+ }
| telemetry_reset
{
telRst();
diff --git a/Control/src/main.cpp b/Control/src/main.cpp
index 658cc1c..a799f4f 100644
--- a/Control/src/main.cpp
+++ b/Control/src/main.cpp
@@ -15,6 +15,7 @@
#include "instruction.h"
#include "colour.h"
#include
+#include "time.h"
#pragma endregion
#pragma region Enable extra debugging info for ESP32
@@ -32,6 +33,7 @@
#define TX3pin 4 // Pin 11 on expansion board, UART3
#define RX4pin 15 // Pin 12 on expansion board, UART4
#define TX4pin 2 // Pin 13 on expansion board, UART4
+#define ntpServer "pool.ntp.org"
#pragma endregion
#pragma region Function Declarations
@@ -73,7 +75,8 @@ int bb_left, bb_right, bb_top, bb_bottom;
int bb_centre_x, bb_centre_y;
float chargeGoal;
int waitGoal;
-Colour_t colour;
+String colour;
+time_t now;
#pragma endregion
void setup()
@@ -124,6 +127,8 @@ void setup()
}
Serial.println("mDNS set up, access Control Panel at 'rover2.local/'");
+ configTime(0, 0, ntpServer);
+
webserver.on("/", HTTP_GET, [](AsyncWebServerRequest *request)
{ request->send(SPIFFS, "/index.html", "text/html"); }); // Serve "index.html" at root page
webserver.on("/command.js", HTTP_GET, [](AsyncWebServerRequest *request)
@@ -204,7 +209,7 @@ void loop()
case INSTR_COLOUR:
{
Status = CS_IDLE;
- colour = (Colour_t)instr->colour;
+ colour = instr->colour;
}
break;
default:
@@ -362,7 +367,8 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length)
Serial.println("Change colour tracking command received");
instr.id = rdoc["Cid"];
instr.instr = INSTR_COLOUR;
- instr.colour = rdoc["col"];
+ String temp = rdoc["col"];
+ instr.colour = temp;
// Ignore rdoc["rH"], rdoc["rD"], rdoc["rS"], rdoc["rC"]
queueInstruction(instr); // Put charge command in InstrQueue
@@ -402,8 +408,10 @@ void sendToCommand()
DynamicJsonDocument tdoc(1024);
tdoc["st"] = Status;
tdoc["bV"] = batteryVoltage;
- tdoc["bL"] = batteryLevel;
- tdoc["bC"] = batteryCycles;
+ tdoc["bL"] = 100; // tdoc["bL"] = batteryLevel; Hardcoded value as Energy is not present on demo Rover
+ time(&now);
+ // Serial.println(now);
+ tdoc["bC"] = ((float)now - 1623000000.0)/21600.0; // tdoc["bC"] = batteryCycles; Estimates cycles in number of 6 hour periods
tdoc["tD"] = odometer;
tdoc["cH"] = heading;
tdoc["pos"][0] = xpos;
@@ -435,6 +443,7 @@ void recvFromDrive() // Update telemetry data and state info from Drive packet
odometer = rdoc["mm"];
xpos = rdoc["pos"][0];
ypos = rdoc["pos"][1];
+ batteryVoltage = rdoc["bV"];
}
}
diff --git a/Drive/src/main.cpp b/Drive/src/main.cpp
index da06bdf..dda6f59 100644
--- a/Drive/src/main.cpp
+++ b/Drive/src/main.cpp
@@ -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,33 @@ void loop()
}
else
{
- //parsing success, prepare command and pull request information
- commandCompletionStatus = 1;
- requiredHeading = rdoc["rH"];
- distance = rdoc["dist"];
- spd = rdoc["sp"];
- currentHeading = rdoc["cH"];
+ 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 = 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
- commandComplete = 0;
- powerUsage_mWh = 0.0;
- distTravelled_mm = 0;
+ //reset variables for update on completion
+ commandComplete = 0;
+ powerUsage_mWh = 0.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);
+ }else if (rdoc.containsKey("stp") && rdoc["stp"] == 1){
+ digitalWrite(pwmr, LOW);
+ digitalWrite(pwml, LOW);
+ commandCompletionStatus = 3;
+ }else if (rdoc.containsKey("rstD") && rdoc["rstD"] == 1){
+ goal_x = 0;
+ goal_y = 0;
+ distTravelled_mm = 0;
+ }
}
}
@@ -464,7 +478,7 @@ void loop()
//Serial.println("status0");
}
if (commandCompletionStatus == 1)
- { Serial.println("status1");
+ { //Serial.println("status1");
//newCommand
//set goals
goal_x += distance * sin(requiredHeading);
@@ -486,17 +500,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 +533,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 +543,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 +561,7 @@ void loop()
}
}
if (commandCompletionStatus == 3)
- { Serial.println("status3");
+ { // Serial.println("status3");
//currentPosMatchesOrExceedsRequest
///finish moving
@@ -557,7 +571,7 @@ void loop()
commandComplete = true;
current_x = goal_x;
current_y = goal_y;
- distTravelled_mm += distance;
+ distTravelled_mm += abs(distance);
//compile energy use
unsigned long currentMillis_Energy = millis();
@@ -585,6 +599,8 @@ void loop()
tdoc["mm"] = distTravelled_mm;
tdoc["pos"][0] = current_x;
tdoc["pos"][1] = current_y;
+ tdoc["bV"] = vb;
+ serializeJson(tdoc, Serial1); // Build JSON and send on UART1
serializeJson(tdoc, Serial); // Build JSON and send on UART1
commandCompletionStatus = 0;
}
@@ -593,7 +609,7 @@ void loop()
//find motor voltage
//int motorVSensor = analogRead(A5);
//float motorVoltage = motorVSensor * (5.0 / 1023.0);
- float motorVoltage = 5;
+ float motorVoltage = vb;
//find average power
@@ -635,7 +651,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 +731,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.
@@ -743,10 +754,11 @@ void sampling(){
// The analogRead process gives a value between 0 and 1023
// representing a voltage between 0 and the analogue reference which is 4.096V
- vb = sensorValue0 * (4.096 / 1023.0); // Convert the Vb sensor reading to volts
+ vb = sensorValue0 * (4.94 / 1023.0); // Convert the Vb sensor reading to volts
vref = sensorValue2 * (4.096 / 1023.0); // Convert the Vref sensor reading to volts
vpd = sensorValue3 * (4.096 / 1023.0); // Convert the Vpd sensor reading to volts
+
// The inductor current is in mA from the sensor so we need to convert to amps.
// We want to treat it as an input current in the Boost, so its also inverted
// For open loop control the duty cycle reference is calculated from the sensor
@@ -831,42 +843,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;
}
\ No newline at end of file
diff --git a/IMU/src/main.cpp b/IMU/src/main.cpp
index c04011f..c2b72b0 100644
--- a/IMU/src/main.cpp
+++ b/IMU/src/main.cpp
@@ -3,6 +3,9 @@
#include
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);
-}
\ No newline at end of file
+
+ delay(52);
+}
+