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