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

Merge pull request #4 from supleed2/Integration

Browse source 
Integration
This commit is contained in:
Aadi Desai 2021-06-15 13:34:49 +01:00 committed by GitHub
parent 6ca4be5e11 7dd064afe9
commit cf85d0750f
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
7 changed files with 191 additions and 120 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

34
Control/data/command.js
View file

@ -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 o=function(k,v,o,l){for(o=o||{},l=k.length;l--;o[k[l]]=v);return o};
var parser = {trace: function trace () { }, var parser = {trace: function trace () { },
yy: {}, 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}, 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:"telemetry_reset",16:"help"}, 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,1],[4,1]], 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 */) { performAction: function anonymous(yytext, yyleng, yylineno, yy, yystate /* action[1] */, $$ /* vstack */, _$ /* lstack */) {
/* this == yyval */ /* this == yyval */
@ -110,18 +110,24 @@ case 5:
break; break;
case 6: case 6:
telRst(); var inColour = String($$[$0]);
colourCmd(inColour);
break; break;
case 7: case 7:
telRst();
break;
case 8:
printHelpDetails(); printHelpDetails();
break; 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]}], 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],7:[2,6],8:[2,7],9:[2,1],14:[2,4],15:[2,5],19:[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) { parseError: function parseError (str, hash) {
if (hash.recoverable) { if (hash.recoverable) {
this.trace(str); this.trace(str);
@ -612,20 +618,24 @@ case 6:return 12
break; break;
case 7:return 11 case 7:return 11
break; break;
case 8:return 16 case 8:return 18
break; break;
case 9:return 14 case 9:return 14
break; break;
case 10:return 15 case 10:return 17
break; break;
case 11:return 5 case 11:return 15
break; break;
case 12:return 'invalid_command' case 12:return 16
break;
case 13:return 5
break;
case 14:return 'invalid_command'
break; 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)/,/^(?:$)/,/^(?:.)/], 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],"inclusive":true}} conditions: {"INITIAL":{"rules":[0,1,2,3,4,5,6,7,8,9,10,11,12,13,14],"inclusive":true}}
}); });
return lexer; return lexer;
})(); })();

57
Control/data/index.html
View file

@ -130,28 +130,28 @@
</tr> </tr>
<tr> <tr>
<td style="text-align: center;" colspan="2"><button style="color: red;">Emergency<br>Stop</button></td> <td style="text-align: center;" colspan="2"><button onclick="setCmdMode(0); stpCmd();" style="color: red;">Emergency<br>Stop</button></td>
<td style="text-align: center;" colspan="2"><button>Telemetry<br>Reset</button></td> <td style="text-align: center;" colspan="2"><button onclick="setCmdMode(-1); telRst();">Telemetry<br>Reset</button></td>
<td style="text-align: center;" colspan="2"><button>Full<br>Charge</button></td> <td style="text-align: center;" colspan="2"><button onclick="setCmdMode(2); chrgCmd(100);">Full<br>Charge</button></td>
</tr> </tr>
<tr><td colspan="6"><hr></td></tr> <tr><td colspan="6"><hr></td></tr>
<tr> <tr>
<td style="text-align: center;" colspan="6"><strong> Set Heading to: </strong><strong id="SetHeading" style="font-size: 18px;">270</strong><strong>&#176;</strong></td> <td style="text-align: center;" colspan="6"><strong> Set Heading to: </strong><strong id="SetHeading" style="font-size: 18px;">270</strong><strong>&#176;</strong></td>
</tr> </tr>
<tr> <tr>
<td colspan="6"><input type ="range" min="0" max="359" value="270" class="slider" id="HdgSlider"></td> <td colspan="6"><input type ="range" min="0" max="359" value="180" class="slider" id="HdgSlider"></td>
</tr> </tr>
<tr> <tr>
<td style="text-align: center;" colspan="6"><strong>Set Translation to: </strong><strong id="SetTrans" style="font-size: 18px;">180</strong><strong>mm</strong></td> <td style="text-align: center;" colspan="6"><strong>Set Translation to: </strong><strong id="SetTrans" style="font-size: 18px;">180</strong><strong>mm</strong></td>
</tr> </tr>
<tr> <tr>
<td colspan="6"><input type ="range" min="0" max="1000" value="100" class="slider" id="TranSlider"></td> <td colspan="6"><input type ="range" min="-200" max="200" value="0" class="slider" id="TranSlider"></td>
</tr> </tr>
<tr> <tr>
<td style="text-align: center;" colspan="6"><strong>Set Speed to: </strong><strong id="SetSpd" style="font-size: 18px;">50</strong><strong>%</strong></td> <td style="text-align: center;" colspan="6"><strong>Set Speed to: </strong><strong id="SetSpd" style="font-size: 18px;">50</strong><strong>%</strong></td>
</tr> </tr>
<tr> <tr>
<td colspan="6"><input type ="range" min="0" max="100" value="50" class="slider" id="SpdSlider"></td> <td colspan="6"><input type ="range" min="0" max="100" value="100" class="slider" id="SpdSlider"></td>
</tr> </tr>
<tr> <tr>
<td colspan="6"><button onclick="setCmdMode(1); moveCmd(setTransto, setHdgto, setSpdto);">Send<br>Command</button></td> <td colspan="6"><button onclick="setCmdMode(1); moveCmd(setTransto, setHdgto, setSpdto);">Send<br>Command</button></td>
@ -235,6 +235,7 @@
var reqSpeed = 0; var reqSpeed = 0;
var reqCharge = 0; var reqCharge = 0;
var pstop_time = 0; var pstop_time = 0;
var hunt_colour = "";
var state = 0; var state = 0;
var batteryVoltage = 0; var batteryVoltage = 0;
@ -255,11 +256,11 @@
var data = JSON.parse(raw_data); var data = JSON.parse(raw_data);
state = data.st; state = data.st;
batteryLevel = data.bV; batteryLevel = data.bL;
batteryVoltage = data.bV; batteryVoltage = data.bV;
batteryCycles = data.bC; batteryCycles = data.bC;
totalTripDistance = data.tD; totalTripDistance = data.tD;
currentHeading = data.cH; currentHeading = 180 - data.cH;
current_pos = data.pos; current_pos = data.pos;
current_x = current_pos[0]; current_x = current_pos[0];
current_y = current_pos[1]; current_y = current_pos[1];
@ -275,6 +276,8 @@
rStatus = "Moving"; rStatus = "Moving";
}else if(state == 2){ }else if(state == 2){
rStatus = "Charging"; rStatus = "Charging";
}else if(state == 3){
rStatus = "Waiting";
}else{ }else{
rStatus = "Undefined"; rStatus = "Undefined";
} }
@ -291,7 +294,7 @@
} }
function send_data() { 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); connection.send(raw_data);
console.log(raw_data); console.log(raw_data);
} }
@ -306,8 +309,16 @@
bufferOutput = document.getElementById("commandInput").value; bufferOutput = document.getElementById("commandInput").value;
}else if(cmdMode == 1){ }else if(cmdMode == 1){
bufferOutput = "move " + reqDistance + "mm " + reqHeading + "deg " + reqSpeed + "%"; 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 + "<br>"; document.getElementById("command_buffer").innerHTML += '[' + command_id + '] ' + bufferOutput + "<br>";
document.getElementById("commandInput").value = "" document.getElementById("commandInput").value = ""
}else if(mode == 0){ }else if(mode == 0){
@ -338,11 +349,12 @@
function moveCmd(dist,hdg,spd){ function moveCmd(dist,hdg,spd){
mode = 1; mode = 1;
reqDistance = dist; reqDistance = dist;
reqHeading = hdg; reqHeading = 360 - hdg;
reqSpeed = spd; reqSpeed = spd;
reqCharge = 0; reqCharge = 0;
pstop_time = 0; pstop_time = 0;
tel_rst = 0; tel_rst = 0;
hunt_colour = "";
send_data(); send_data();
updateCommandBuffer(); updateCommandBuffer();
command_id++; command_id++;
@ -356,6 +368,7 @@
reqCharge = 0; reqCharge = 0;
pstop_time = 0; pstop_time = 0;
tel_rst = 0; tel_rst = 0;
hunt_colour = "";
send_data(); send_data();
command_id = 1; command_id = 1;
updateCommandBuffer(); updateCommandBuffer();
@ -369,6 +382,7 @@
reqCharge = 0; reqCharge = 0;
pstop_time = pstp_tme; pstop_time = pstp_tme;
tel_rst = 0; tel_rst = 0;
hunt_colour = "";
send_data(); send_data();
updateCommandBuffer(); updateCommandBuffer();
command_id++; command_id++;
@ -382,6 +396,7 @@
reqCharge = chrglvl; reqCharge = chrglvl;
pstop_time = 0; pstop_time = 0;
tel_rst = 0; tel_rst = 0;
hunt_colour = "";
send_data(); send_data();
updateCommandBuffer(); updateCommandBuffer();
command_id++; command_id++;
@ -394,6 +409,20 @@
reqSpeed = 0; reqSpeed = 0;
reqCharge = 0; reqCharge = 0;
pstop_time = 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(); send_data();
updateCommandBuffer(); updateCommandBuffer();
command_id++; command_id++;
@ -409,7 +438,7 @@
} }
} }
var setHdgto = 270; var setHdgto = 180;
var hdg_slider = document.getElementById("HdgSlider"); var hdg_slider = document.getElementById("HdgSlider");
var hdg_output = document.getElementById("SetHeading"); var hdg_output = document.getElementById("SetHeading");
hdg_output.innerHTML = hdg_slider.value; hdg_output.innerHTML = hdg_slider.value;
@ -418,7 +447,7 @@
setHdgto = this.value; setHdgto = this.value;
} }
var setTransto = 100; var setTransto = 0;
var tran_slider = document.getElementById("TranSlider"); var tran_slider = document.getElementById("TranSlider");
var tran_output = document.getElementById("SetTrans"); var tran_output = document.getElementById("SetTrans");
tran_output.innerHTML = tran_slider.value; tran_output.innerHTML = tran_slider.value;
@ -427,7 +456,7 @@
setTransto = this.value; setTransto = this.value;
} }
var setSpdto = 50; var setSpdto = 100;
var spd_slider = document.getElementById("SpdSlider"); var spd_slider = document.getElementById("SpdSlider");
var spd_output = document.getElementById("SetSpd"); var spd_output = document.getElementById("SetSpd");
spd_output.innerHTML = spd_slider.value; spd_output.innerHTML = spd_slider.value;

3
Control/include/instruction.h
View file

@ -1,5 +1,6 @@
#ifndef INSTRUCTION_H #ifndef INSTRUCTION_H
#define INSTRUCTION_H #define INSTRUCTION_H
#include <string>
typedef enum { typedef enum {
INSTR_RESET = -1, INSTR_RESET = -1,
@ -19,7 +20,7 @@ typedef struct instruction
float speed; float speed;
int charge; int charge;
int time; int time;
int colour; String colour;
} RoverInstruction; } RoverInstruction;
#endif #endif

27
Control/src/command.jison
View file

@ -2,16 +2,18 @@
%% %%
\s return 'whitespace' \s return 'whitespace'
\b[0-9]+"mm"\b return 'distance' ("-"[0-9]+"mm"|[0-9]+"mm") 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' ([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'
\b([0-9]|[1-8][0-9]|9[0-9]|100)"%" return 'percentage' ([0-9]|[1-8][0-9]|9[0-9]|100)"%" return 'percentage'
\b[0-9]+"s"\b return 'stop_duration' [0-9]+"s" return 'stop_duration'
\bmove\b return 'move' move return 'move'
\bpstop\b return 'pstop' pstop return 'pstop'
\bstop\b return 'stop' stop return 'stop'
\bhelp\b return 'help' help return 'help'
\bcharge\sto\b return 'charge_to' charge\sto return 'charge_to'
\btelemetry\sreset\b return 'telemetry_reset' telemetry\sreset return 'telemetry_reset'
colour return 'colour'
("red"|"blue"|"green"|"pink"|"orange") return 'colour_name'
<<EOF>> return 'EOF' <<EOF>> return 'EOF'
. return 'invalid_command' . return 'invalid_command'
@ -47,6 +49,11 @@ expr
var inChrg = Number(String($3).substr(0, ((String($3).length) - 1))); var inChrg = Number(String($3).substr(0, ((String($3).length) - 1)));
chrgCmd(inChrg); chrgCmd(inChrg);
} }
| colour whitespace colour_name
{
var inColour = String($3);
colourCmd(inColour);
}
| telemetry_reset | telemetry_reset
{ {
telRst(); telRst();

19
Control/src/main.cpp
View file

@ -15,6 +15,7 @@
#include "instruction.h" #include "instruction.h"
#include "colour.h" #include "colour.h"
#include <queue> #include <queue>
#include "time.h"
#pragma endregion #pragma endregion
#pragma region Enable extra debugging info for ESP32 #pragma region Enable extra debugging info for ESP32
@ -32,6 +33,7 @@
#define TX3pin 4 // Pin 11 on expansion board, UART3 #define TX3pin 4 // Pin 11 on expansion board, UART3
#define RX4pin 15 // Pin 12 on expansion board, UART4 #define RX4pin 15 // Pin 12 on expansion board, UART4
#define TX4pin 2 // Pin 13 on expansion board, UART4 #define TX4pin 2 // Pin 13 on expansion board, UART4
#define ntpServer "pool.ntp.org"
#pragma endregion #pragma endregion
#pragma region Function Declarations #pragma region Function Declarations
@ -73,7 +75,8 @@ int bb_left, bb_right, bb_top, bb_bottom;
int bb_centre_x, bb_centre_y; int bb_centre_x, bb_centre_y;
float chargeGoal; float chargeGoal;
int waitGoal; int waitGoal;
Colour_t colour; String colour;
time_t now;
#pragma endregion #pragma endregion
void setup() void setup()
@ -124,6 +127,8 @@ void setup()
} }
Serial.println("mDNS set up, access Control Panel at 'rover2.local/'"); Serial.println("mDNS set up, access Control Panel at 'rover2.local/'");
configTime(0, 0, ntpServer);
webserver.on("/", HTTP_GET, [](AsyncWebServerRequest *request) webserver.on("/", HTTP_GET, [](AsyncWebServerRequest *request)
{ request->send(SPIFFS, "/index.html", "text/html"); }); // Serve "index.html" at root page { request->send(SPIFFS, "/index.html", "text/html"); }); // Serve "index.html" at root page
webserver.on("/command.js", HTTP_GET, [](AsyncWebServerRequest *request) webserver.on("/command.js", HTTP_GET, [](AsyncWebServerRequest *request)
@ -204,7 +209,7 @@ void loop()
case INSTR_COLOUR: case INSTR_COLOUR:
{ {
Status = CS_IDLE; Status = CS_IDLE;
colour = (Colour_t)instr->colour; colour = instr->colour;
} }
break; break;
default: 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"); Serial.println("Change colour tracking command received");
instr.id = rdoc["Cid"]; instr.id = rdoc["Cid"];
instr.instr = INSTR_COLOUR; instr.instr = INSTR_COLOUR;
instr.colour = rdoc["col"]; String temp = rdoc["col"];
instr.colour = temp;
// Ignore rdoc["rH"], rdoc["rD"], rdoc["rS"], rdoc["rC"] // Ignore rdoc["rH"], rdoc["rD"], rdoc["rS"], rdoc["rC"]
queueInstruction(instr); // Put charge command in InstrQueue queueInstruction(instr); // Put charge command in InstrQueue
@ -402,8 +408,10 @@ void sendToCommand()
DynamicJsonDocument tdoc(1024); DynamicJsonDocument tdoc(1024);
tdoc["st"] = Status; tdoc["st"] = Status;
tdoc["bV"] = batteryVoltage; tdoc["bV"] = batteryVoltage;
tdoc["bL"] = batteryLevel; tdoc["bL"] = 100; // tdoc["bL"] = batteryLevel; Hardcoded value as Energy is not present on demo Rover
tdoc["bC"] = batteryCycles; 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["tD"] = odometer;
tdoc["cH"] = heading; tdoc["cH"] = heading;
tdoc["pos"][0] = xpos; tdoc["pos"][0] = xpos;
@ -435,6 +443,7 @@ void recvFromDrive() // Update telemetry data and state info from Drive packet
odometer = rdoc["mm"]; odometer = rdoc["mm"];
xpos = rdoc["pos"][0]; xpos = rdoc["pos"][0];
ypos = rdoc["pos"][1]; ypos = rdoc["pos"][1];
batteryVoltage = rdoc["bV"];
} }
} }

124
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,21 +438,35 @@ void loop()
} }
else else
{ {
if(rdoc.containsKey("sp") && rdoc["rH"] != -1){
//parsing success, prepare command and pull request information //parsing success, prepare command and pull request information
commandCompletionStatus = 1; commandCompletionStatus = 1;
requiredHeading = rdoc["rH"]; requiredHeading = rdoc["rH"];
distance = rdoc["dist"]; distance = rdoc["dist"];
spd = rdoc["sp"]; spd = rdoc["sp"];
currentHeading = rdoc["cH"]; 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;
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; distTravelled_mm = 0;
} }
} }
}
//if(current_x!=goal_x) //if(current_x!=goal_x)
@ -464,7 +478,7 @@ void loop()
//Serial.println("status0"); //Serial.println("status0");
} }
if (commandCompletionStatus == 1) if (commandCompletionStatus == 1)
{ Serial.println("status1"); { //Serial.println("status1");
//newCommand //newCommand
//set goals //set goals
goal_x += distance * sin(requiredHeading); goal_x += distance * sin(requiredHeading);
@ -486,17 +500,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 +533,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 +543,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 +561,7 @@ void loop()
} }
} }
if (commandCompletionStatus == 3) if (commandCompletionStatus == 3)
{ Serial.println("status3"); { // Serial.println("status3");
//currentPosMatchesOrExceedsRequest //currentPosMatchesOrExceedsRequest
///finish moving ///finish moving
@ -557,7 +571,7 @@ void loop()
commandComplete = true; commandComplete = true;
current_x = goal_x; current_x = goal_x;
current_y = goal_y; current_y = goal_y;
distTravelled_mm += distance; distTravelled_mm += abs(distance);
//compile energy use //compile energy use
unsigned long currentMillis_Energy = millis(); unsigned long currentMillis_Energy = millis();
@ -585,6 +599,8 @@ 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;
tdoc["bV"] = vb;
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;
} }
@ -593,7 +609,7 @@ void loop()
//find motor voltage //find motor voltage
//int motorVSensor = analogRead(A5); //int motorVSensor = analogRead(A5);
//float motorVoltage = motorVSensor * (5.0 / 1023.0); //float motorVoltage = motorVSensor * (5.0 / 1023.0);
float motorVoltage = 5; float motorVoltage = vb;
//find average power //find average power
@ -635,7 +651,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 +731,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.
@ -743,10 +754,11 @@ void sampling(){
// The analogRead process gives a value between 0 and 1023 // The analogRead process gives a value between 0 and 1023
// representing a voltage between 0 and the analogue reference which is 4.096V // 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 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 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. // 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 // 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 // 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 // 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;
} }

19
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);
} }