mirror of
https://github.com/supleed2/ELEC50003-P1-CW.git
synced 2024-12-22 21:45:49 +00:00
GitHub
commit
cf85d0750f
|
|
@ -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;
|
||||
})();
|
||||
|
|
|
|||
|
|
@ -130,28 +130,28 @@
|
|||
</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>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(0); stpCmd();" style="color: red;">Emergency<br>Stop</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 onclick="setCmdMode(2); chrgCmd(100);">Full<br>Charge</button></td>
|
||||
</tr>
|
||||
<tr><td colspan="6"><hr></td></tr>
|
||||
<tr>
|
||||
<td style="text-align: center;" colspan="6"><strong> Set Heading to: </strong><strong id="SetHeading" style="font-size: 18px;">270</strong><strong>°</strong></td>
|
||||
</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>
|
||||
<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>
|
||||
<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>
|
||||
<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>
|
||||
<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>
|
||||
<td colspan="6"><button onclick="setCmdMode(1); moveCmd(setTransto, setHdgto, setSpdto);">Send<br>Command</button></td>
|
||||
|
|
@ -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 + "<br>";
|
||||
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;
|
||||
|
|
|
|||
|
|
@ -1,5 +1,6 @@
|
|||
#ifndef INSTRUCTION_H
|
||||
#define INSTRUCTION_H
|
||||
#include <string>
|
||||
|
||||
typedef enum {
|
||||
INSTR_RESET = -1,
|
||||
|
|
@ -19,7 +20,7 @@ typedef struct instruction
|
|||
float speed;
|
||||
int charge;
|
||||
int time;
|
||||
int colour;
|
||||
String colour;
|
||||
} RoverInstruction;
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -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'
|
||||
<<EOF>> 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'
|
||||
<<EOF>> 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();
|
||||
|
|
|
|||
|
|
@ -15,6 +15,7 @@
|
|||
#include "instruction.h"
|
||||
#include "colour.h"
|
||||
#include <queue>
|
||||
#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"];
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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