Merge branch 'main' into Testad3919

This commit is contained in:
Aadi Desai 2021-06-15 03:08:12 +01:00
commit bbe36fb9dd
20 changed files with 2275 additions and 247 deletions

656
Control/data/command.js Normal file
View file

@ -0,0 +1,656 @@
/* parser generated by jison 0.4.18 */
/*
Returns a Parser object of the following structure:
Parser: {
yy: {}
}
Parser.prototype: {
yy: {},
trace: function(),
symbols_: {associative list: name ==> number},
terminals_: {associative list: number ==> name},
productions_: [...],
performAction: function anonymous(yytext, yyleng, yylineno, yy, yystate, $$, _$),
table: [...],
defaultActions: {...},
parseError: function(str, hash),
parse: function(input),
lexer: {
EOF: 1,
parseError: function(str, hash),
setInput: function(input),
input: function(),
unput: function(str),
more: function(),
less: function(n),
pastInput: function(),
upcomingInput: function(),
showPosition: function(),
test_match: function(regex_match_array, rule_index),
next: function(),
lex: function(),
begin: function(condition),
popState: function(),
_currentRules: function(),
topState: function(),
pushState: function(condition),
options: {
ranges: boolean (optional: true ==> token location info will include a .range[] member)
flex: boolean (optional: true ==> flex-like lexing behaviour where the rules are tested exhaustively to find the longest match)
backtrack_lexer: boolean (optional: true ==> lexer regexes are tested in order and for each matching regex the action code is invoked; the lexer terminates the scan when a token is returned by the action code)
},
performAction: function(yy, yy_, $avoiding_name_collisions, YY_START),
rules: [...],
conditions: {associative list: name ==> set},
}
}
token location info (@$, _$, etc.): {
first_line: n,
last_line: n,
first_column: n,
last_column: n,
range: [start_number, end_number] (where the numbers are indexes into the input string, regular zero-based)
}
the parseError function receives a 'hash' object with these members for lexer and parser errors: {
text: (matched text)
token: (the produced terminal token, if any)
line: (yylineno)
}
while parser (grammar) errors will also provide these members, i.e. parser errors deliver a superset of attributes: {
loc: (yylloc)
expected: (string describing the set of expected tokens)
recoverable: (boolean: TRUE when the parser has a error recovery rule available for this particular error)
}
*/
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]],
performAction: function anonymous(yytext, yyleng, yylineno, yy, yystate /* action[1] */, $$ /* vstack */, _$ /* lstack */) {
/* this == yyval */
var $0 = $$.length - 1;
switch (yystate) {
case 2:
var inDist = Number(String($$[$0-4]).substr(0, ((String($$[$0-4]).length) - 2)));
var inHdg = Number(String($$[$0-2]).substr(0, ((String($$[$0-2]).length) - 3)));
var inSpd = Number(String($$[$0]).substr(0, ((String($$[$0]).length) - 1)));
moveCmd(inDist,inHdg,inSpd);
break;
case 3:
stpCmd();
break;
case 4:
var inStpDur = Number(String($$[$0]).substr(0, ((String($$[$0]).length) - 1)));
pstpCmd(inStpDur);
break;
case 5:
var inChrg = Number(String($$[$0]).substr(0, ((String($$[$0]).length) - 1)));
chrgCmd(inChrg);
break;
case 6:
telRst();
break;
case 7:
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]},
parseError: function parseError (str, hash) {
if (hash.recoverable) {
this.trace(str);
} else {
var error = new Error(str);
error.hash = hash;
throw error;
}
},
parse: function parse(input) {
var self = this, stack = [0], tstack = [], vstack = [null], lstack = [], table = this.table, yytext = '', yylineno = 0, yyleng = 0, recovering = 0, TERROR = 2, EOF = 1;
var args = lstack.slice.call(arguments, 1);
var lexer = Object.create(this.lexer);
var sharedState = { yy: {} };
for (var k in this.yy) {
if (Object.prototype.hasOwnProperty.call(this.yy, k)) {
sharedState.yy[k] = this.yy[k];
}
}
lexer.setInput(input, sharedState.yy);
sharedState.yy.lexer = lexer;
sharedState.yy.parser = this;
if (typeof lexer.yylloc == 'undefined') {
lexer.yylloc = {};
}
var yyloc = lexer.yylloc;
lstack.push(yyloc);
var ranges = lexer.options && lexer.options.ranges;
if (typeof sharedState.yy.parseError === 'function') {
this.parseError = sharedState.yy.parseError;
} else {
this.parseError = Object.getPrototypeOf(this).parseError;
}
function popStack(n) {
stack.length = stack.length - 2 * n;
vstack.length = vstack.length - n;
lstack.length = lstack.length - n;
}
_token_stack:
var lex = function () {
var token;
token = lexer.lex() || EOF;
if (typeof token !== 'number') {
token = self.symbols_[token] || token;
}
return token;
};
var symbol, preErrorSymbol, state, action, a, r, yyval = {}, p, len, newState, expected;
while (true) {
state = stack[stack.length - 1];
if (this.defaultActions[state]) {
action = this.defaultActions[state];
} else {
if (symbol === null || typeof symbol == 'undefined') {
symbol = lex();
}
action = table[state] && table[state][symbol];
}
if (typeof action === 'undefined' || !action.length || !action[0]) {
var errStr = '';
expected = [];
for (p in table[state]) {
if (this.terminals_[p] && p > TERROR) {
expected.push('\'' + this.terminals_[p] + '\'');
}
}
if (lexer.showPosition) {
errStr = 'Parse error on line ' + (yylineno + 1) + ':\n' + lexer.showPosition() + '\nExpecting ' + expected.join(', ') + ', got \'' + (this.terminals_[symbol] || symbol) + '\'';
} else {
errStr = 'Parse error on line ' + (yylineno + 1) + ': Unexpected ' + (symbol == EOF ? 'end of input' : '\'' + (this.terminals_[symbol] || symbol) + '\'');
}
this.parseError(errStr, {
text: lexer.match,
token: this.terminals_[symbol] || symbol,
line: lexer.yylineno,
loc: yyloc,
expected: expected
});
}
if (action[0] instanceof Array && action.length > 1) {
throw new Error('Parse Error: multiple actions possible at state: ' + state + ', token: ' + symbol);
}
switch (action[0]) {
case 1:
stack.push(symbol);
vstack.push(lexer.yytext);
lstack.push(lexer.yylloc);
stack.push(action[1]);
symbol = null;
if (!preErrorSymbol) {
yyleng = lexer.yyleng;
yytext = lexer.yytext;
yylineno = lexer.yylineno;
yyloc = lexer.yylloc;
if (recovering > 0) {
recovering--;
}
} else {
symbol = preErrorSymbol;
preErrorSymbol = null;
}
break;
case 2:
len = this.productions_[action[1]][1];
yyval.$ = vstack[vstack.length - len];
yyval._$ = {
first_line: lstack[lstack.length - (len || 1)].first_line,
last_line: lstack[lstack.length - 1].last_line,
first_column: lstack[lstack.length - (len || 1)].first_column,
last_column: lstack[lstack.length - 1].last_column
};
if (ranges) {
yyval._$.range = [
lstack[lstack.length - (len || 1)].range[0],
lstack[lstack.length - 1].range[1]
];
}
r = this.performAction.apply(yyval, [
yytext,
yyleng,
yylineno,
sharedState.yy,
action[1],
vstack,
lstack
].concat(args));
if (typeof r !== 'undefined') {
return r;
}
if (len) {
stack = stack.slice(0, -1 * len * 2);
vstack = vstack.slice(0, -1 * len);
lstack = lstack.slice(0, -1 * len);
}
stack.push(this.productions_[action[1]][0]);
vstack.push(yyval.$);
lstack.push(yyval._$);
newState = table[stack[stack.length - 2]][stack[stack.length - 1]];
stack.push(newState);
break;
case 3:
return true;
}
}
return true;
}};
/* generated by jison-lex 0.3.4 */
var lexer = (function(){
var lexer = ({
EOF:1,
parseError:function parseError(str, hash) {
if (this.yy.parser) {
this.yy.parser.parseError(str, hash);
} else {
throw new Error(str);
}
},
// resets the lexer, sets new input
setInput:function (input, yy) {
this.yy = yy || this.yy || {};
this._input = input;
this._more = this._backtrack = this.done = false;
this.yylineno = this.yyleng = 0;
this.yytext = this.matched = this.match = '';
this.conditionStack = ['INITIAL'];
this.yylloc = {
first_line: 1,
first_column: 0,
last_line: 1,
last_column: 0
};
if (this.options.ranges) {
this.yylloc.range = [0,0];
}
this.offset = 0;
return this;
},
// consumes and returns one char from the input
input:function () {
var ch = this._input[0];
this.yytext += ch;
this.yyleng++;
this.offset++;
this.match += ch;
this.matched += ch;
var lines = ch.match(/(?:\r\n?|\n).*/g);
if (lines) {
this.yylineno++;
this.yylloc.last_line++;
} else {
this.yylloc.last_column++;
}
if (this.options.ranges) {
this.yylloc.range[1]++;
}
this._input = this._input.slice(1);
return ch;
},
// unshifts one char (or a string) into the input
unput:function (ch) {
var len = ch.length;
var lines = ch.split(/(?:\r\n?|\n)/g);
this._input = ch + this._input;
this.yytext = this.yytext.substr(0, this.yytext.length - len);
//this.yyleng -= len;
this.offset -= len;
var oldLines = this.match.split(/(?:\r\n?|\n)/g);
this.match = this.match.substr(0, this.match.length - 1);
this.matched = this.matched.substr(0, this.matched.length - 1);
if (lines.length - 1) {
this.yylineno -= lines.length - 1;
}
var r = this.yylloc.range;
this.yylloc = {
first_line: this.yylloc.first_line,
last_line: this.yylineno + 1,
first_column: this.yylloc.first_column,
last_column: lines ?
(lines.length === oldLines.length ? this.yylloc.first_column : 0)
+ oldLines[oldLines.length - lines.length].length - lines[0].length :
this.yylloc.first_column - len
};
if (this.options.ranges) {
this.yylloc.range = [r[0], r[0] + this.yyleng - len];
}
this.yyleng = this.yytext.length;
return this;
},
// When called from action, caches matched text and appends it on next action
more:function () {
this._more = true;
return this;
},
// When called from action, signals the lexer that this rule fails to match the input, so the next matching rule (regex) should be tested instead.
reject:function () {
if (this.options.backtrack_lexer) {
this._backtrack = true;
} else {
return this.parseError('Lexical error on line ' + (this.yylineno + 1) + '. You can only invoke reject() in the lexer when the lexer is of the backtracking persuasion (options.backtrack_lexer = true).\n' + this.showPosition(), {
text: "",
token: null,
line: this.yylineno
});
}
return this;
},
// retain first n characters of the match
less:function (n) {
this.unput(this.match.slice(n));
},
// displays already matched input, i.e. for error messages
pastInput:function () {
var past = this.matched.substr(0, this.matched.length - this.match.length);
return (past.length > 20 ? '...':'') + past.substr(-20).replace(/\n/g, "");
},
// displays upcoming input, i.e. for error messages
upcomingInput:function () {
var next = this.match;
if (next.length < 20) {
next += this._input.substr(0, 20-next.length);
}
return (next.substr(0,20) + (next.length > 20 ? '...' : '')).replace(/\n/g, "");
},
// displays the character position where the lexing error occurred, i.e. for error messages
showPosition:function () {
var pre = this.pastInput();
var c = new Array(pre.length + 1).join("-");
return pre + this.upcomingInput() + "\n" + c + "^";
},
// test the lexed token: return FALSE when not a match, otherwise return token
test_match:function(match, indexed_rule) {
var token,
lines,
backup;
if (this.options.backtrack_lexer) {
// save context
backup = {
yylineno: this.yylineno,
yylloc: {
first_line: this.yylloc.first_line,
last_line: this.last_line,
first_column: this.yylloc.first_column,
last_column: this.yylloc.last_column
},
yytext: this.yytext,
match: this.match,
matches: this.matches,
matched: this.matched,
yyleng: this.yyleng,
offset: this.offset,
_more: this._more,
_input: this._input,
yy: this.yy,
conditionStack: this.conditionStack.slice(0),
done: this.done
};
if (this.options.ranges) {
backup.yylloc.range = this.yylloc.range.slice(0);
}
}
lines = match[0].match(/(?:\r\n?|\n).*/g);
if (lines) {
this.yylineno += lines.length;
}
this.yylloc = {
first_line: this.yylloc.last_line,
last_line: this.yylineno + 1,
first_column: this.yylloc.last_column,
last_column: lines ?
lines[lines.length - 1].length - lines[lines.length - 1].match(/\r?\n?/)[0].length :
this.yylloc.last_column + match[0].length
};
this.yytext += match[0];
this.match += match[0];
this.matches = match;
this.yyleng = this.yytext.length;
if (this.options.ranges) {
this.yylloc.range = [this.offset, this.offset += this.yyleng];
}
this._more = false;
this._backtrack = false;
this._input = this._input.slice(match[0].length);
this.matched += match[0];
token = this.performAction.call(this, this.yy, this, indexed_rule, this.conditionStack[this.conditionStack.length - 1]);
if (this.done && this._input) {
this.done = false;
}
if (token) {
return token;
} else if (this._backtrack) {
// recover context
for (var k in backup) {
this[k] = backup[k];
}
return false; // rule action called reject() implying the next rule should be tested instead.
}
return false;
},
// return next match in input
next:function () {
if (this.done) {
return this.EOF;
}
if (!this._input) {
this.done = true;
}
var token,
match,
tempMatch,
index;
if (!this._more) {
this.yytext = '';
this.match = '';
}
var rules = this._currentRules();
for (var i = 0; i < rules.length; i++) {
tempMatch = this._input.match(this.rules[rules[i]]);
if (tempMatch && (!match || tempMatch[0].length > match[0].length)) {
match = tempMatch;
index = i;
if (this.options.backtrack_lexer) {
token = this.test_match(tempMatch, rules[i]);
if (token !== false) {
return token;
} else if (this._backtrack) {
match = false;
continue; // rule action called reject() implying a rule MISmatch.
} else {
// else: this is a lexer rule which consumes input without producing a token (e.g. whitespace)
return false;
}
} else if (!this.options.flex) {
break;
}
}
}
if (match) {
token = this.test_match(match, rules[index]);
if (token !== false) {
return token;
}
// else: this is a lexer rule which consumes input without producing a token (e.g. whitespace)
return false;
}
if (this._input === "") {
return this.EOF;
} else {
return this.parseError('Lexical error on line ' + (this.yylineno + 1) + '. Unrecognized text.\n' + this.showPosition(), {
text: "",
token: null,
line: this.yylineno
});
}
},
// return next match that has a token
lex:function lex () {
var r = this.next();
if (r) {
return r;
} else {
return this.lex();
}
},
// activates a new lexer condition state (pushes the new lexer condition state onto the condition stack)
begin:function begin (condition) {
this.conditionStack.push(condition);
},
// pop the previously active lexer condition state off the condition stack
popState:function popState () {
var n = this.conditionStack.length - 1;
if (n > 0) {
return this.conditionStack.pop();
} else {
return this.conditionStack[0];
}
},
// produce the lexer rule set which is active for the currently active lexer condition state
_currentRules:function _currentRules () {
if (this.conditionStack.length && this.conditionStack[this.conditionStack.length - 1]) {
return this.conditions[this.conditionStack[this.conditionStack.length - 1]].rules;
} else {
return this.conditions["INITIAL"].rules;
}
},
// return the currently active lexer condition state; when an index argument is provided it produces the N-th previous condition state, if available
topState:function topState (n) {
n = this.conditionStack.length - 1 - Math.abs(n || 0);
if (n >= 0) {
return this.conditionStack[n];
} else {
return "INITIAL";
}
},
// alias for begin(condition)
pushState:function pushState (condition) {
this.begin(condition);
},
// return the number of states currently on the stack
stateStackSize:function stateStackSize() {
return this.conditionStack.length;
},
options: {},
performAction: function anonymous(yy,yy_,$avoiding_name_collisions,YY_START) {
var YYSTATE=YY_START;
switch($avoiding_name_collisions) {
case 0:return 7
break;
case 1:return 8
break;
case 2:return 9
break;
case 3:return 10
break;
case 4:return 13
break;
case 5:return 6
break;
case 6:return 12
break;
case 7:return 11
break;
case 8:return 16
break;
case 9:return 14
break;
case 10:return 15
break;
case 11:return 5
break;
case 12: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}}
});
return lexer;
})();
parser.lexer = lexer;
function Parser () {
this.yy = {};
}
Parser.prototype = parser;parser.Parser = Parser;
return new Parser;
})();
if (typeof require !== 'undefined' && typeof exports !== 'undefined') {
exports.parser = command;
exports.Parser = command.Parser;
exports.parse = function () { return command.parse.apply(command, arguments); };
exports.main = function commonjsMain (args) {
if (!args[1]) {
console.log('Usage: '+args[0]+' FILE');
process.exit(1);
}
var source = require('fs').readFileSync(require('path').normalize(args[1]), "utf8");
return exports.parser.parse(source);
};
if (typeof module !== 'undefined' && require.main === module) {
exports.main(process.argv.slice(1));
}
}

View file

@ -12,82 +12,91 @@
box-sizing: border-box;
}
.section_container {
float: left;
width: 50%;
padding: 10px;
}
.flex-container {
display: flex;
flex-wrap: nowrap;
}
ul {
list-style-type: none;
margin: 0;
padding: 0;
}
li {
padding: 0px;
margin-bottom: 0px;
}
:is(h1, h2, h3, h4, h5, h6, label, strong, meter) {
font-family: Arial, Helvetica, sans-serif;
}
.movement_control {
text-align: center;
h2{
margin: 0px;
}
.sensor_data {
text-align: center;
.slider {
-webkit-appearance: none;
width: 100%;
height: 25px;
background: #d3d3d3;
outline: none;
opacity: 0.7;
-webkit-transition: .2s;
transition: opacity .2s;
}
.slider:hover {
opacity: 1;
}
.slider::-webkit-slider-thumb {
-webkit-appearance: none;
appearance: none;
width: 25px;
height: 25px;
background: #000000;
cursor: pointer;
}
#command_space {
width:100%;
height: 200px;
line-height: normal;
overflow: auto;
background-color: rgb(248, 248, 248);
color: black;
border: 1px solid black;
}
meter {
width: 100%;
height: 40px;
transform: translateY(-8px);
transform: translateY(8px);
}
meter::after {
content: attr(value) attr(title);
top: -28px;
left: 0px;
left: 45%;
position: relative;
}
.button {
button {
width: 100%;
display: inline-block;
padding: 15px 25px;
font-size: 24px;
padding: 7px 7px;
font-size: 12px;
cursor: pointer;
text-align: center;
text-decoration: none;
outline: none;
color: rgb(255, 255, 255);
background-color: #161616;
background-color: #222222;
border: none;
border-radius: 15px;
box-shadow: 0 9px rgb(161, 161, 161);
border-radius: 5px;
box-shadow: 0 3px rgb(161, 161, 161);
-webkit-transition: .2s;
transition: background-color .2s;
}
.button:hover {
button:hover {
background-color: #585858
}
.button:active {
background-color: #107C10;
box-shadow: 0 5px rgb(161, 161, 161);
transform: translateY(4px);
}
.pressed {
background-color: #107C10;
box-shadow: 0 5px rgb(161, 161, 161);
transform: translateY(4px);
button:active {
box-shadow: 0 3px rgb(161, 161, 161);
transform: translateY(1px);
}
.clearfix::after {
@ -96,203 +105,337 @@
display: table;
}
</style>
<script>
var connection = new WebSocket('ws://' + location.hostname + ':81/');
var MVM_F_status = 0;
var MVM_L_status = 0;
var MVM_R_status = 0;
var MVM_B_status = 0;
var BTRY_VOLT = 0;
var ODO_DIST = 0;
connection.onmessage = function (event) {
var raw_data = event.data;
console.log(raw_data);
var data = JSON.parse(raw_data);
BTRY_VOLT = data.BTRY_VOLT;
ODO_DIST = data.ODO_DIST;
document.getElementById("btry_meter").value = BTRY_VOLT;
document.getElementById("Odometer").innerHTML = ODO_DIST;
}
function send_data() {
var raw_data = '{"MVM_F":' + MVM_F_status + ',"MVM_L":' + MVM_L_status + ',"MVM_R":' + MVM_R_status + ',"MVM_B":' + MVM_B_status + '}';
connection.send(raw_data);
console.log(raw_data);
}
function left_pressed() {
MVM_L_status = 1;
send_data();
}
function left_unpressed() {
MVM_L_status = 0;
send_data();
}
function up_pressed() {
MVM_F_status = 1;
send_data();
}
function up_unpressed() {
MVM_F_status = 0;
send_data();
}
function right_pressed() {
MVM_R_status = 1;
send_data();
}
function right_unpressed() {
MVM_R_status = 0;
send_data();
}
function down_pressed() {
MVM_B_status = 1;
send_data();
}
function down_unpressed() {
MVM_B_status = 0;
send_data();
}
var timer = null;
function up_mouseDown() {
timer = setInterval(up_pressed, 100);
}
function up_mouseUp() {
clearInterval(timer);
up_unpressed();
}
function down_mouseDown() {
timer = setInterval(down_pressed, 100);
}
function down_mouseUp() {
clearInterval(timer);
down_unpressed();
}
function right_mouseDown() {
timer = setInterval(right_pressed, 100);
}
function right_mouseUp() {
clearInterval(timer);
right_unpressed();
}
function left_mouseDown() {
timer = setInterval(left_pressed, 100);
}
function left_mouseUp() {
clearInterval(timer);
left_unpressed();
}
document.onkeydown = function (e) {
switch (e.keyCode) {
case 37:
document.getElementById("left_arrow").className = "button pressed";
left_pressed();
break;
case 38:
document.getElementById("up_arrow").className = "button pressed";
up_pressed();
break;
case 39:
document.getElementById("right_arrow").className = "button pressed";
right_pressed();
break;
case 40:
document.getElementById("down_arrow").className = "button pressed";
down_pressed();
break;
}
};
document.onkeyup = function (e) {
switch (e.keyCode) {
case 37:
document.getElementById("left_arrow").className = "button";
left_unpressed();
break;
case 38:
document.getElementById("up_arrow").className = "button";
up_unpressed();
break;
case 39:
document.getElementById("right_arrow").className = "button";
right_unpressed();
break;
case 40:
document.getElementById("down_arrow").className = "button";
down_unpressed();
break;
}
};
</script>
</head>
<body>
<h1 style="text-align:center;">ROVER COMMAND CENTER</h1>
<div class="clearfix">
<table style="width:1100px; border-spacing: 10px; margin-left: auto; margin-right: auto;">
<tr>
<th style="width: 33%;"><h2>Control Panel</h2></th>
<th style="width: 33%;"><h2>Telemetry</h2></th>
<th style="width: 33%;"><h2>Command Console</h2></th>
</tr>
<tr><hr></tr>
<div class="section_container">
<div class="movement_control">
<h2>Movement Control</h2>
<div style="transform: translateY(0px);">
<button id="up_arrow" onmousedown="up_mouseDown()" onmouseup="up_mouseUp()"
class="button"><span>&#8679;</span></button>
</div>
<div style="transform: translateY(13px);">
<button id="left_arrow" onmousedown="left_mouseDown()" onmouseup="left_mouseUp()"
class="button"><span>&#8678;</span></button>
<button id="down_arrow" onmousedown="down_mouseDown()" onmouseup="down_mouseUp()"
class="button"><span>&#8681;</span></button>
<button id="right_arrow" onmousedown="right_mouseDown()" onmouseup="right_mouseUp()"
class="button"><span>&#8680;</span></button>
</div>
<!-- Control Panel Section -->
<tr>
<td style="vertical-align: top;">
<table style="width:100%; border-spacing: 5px;">
<tr><td colspan="6"><hr></td></tr>
<tr>
<td style="text-align: center;" colspan="6"><strong>Main</strong></td>
</tr>
</div>
</div>
<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>
</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>&#176;</strong></td>
</tr>
<tr>
<td colspan="6"><input type ="range" min="0" max="359" value="270" 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>
</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>
</tr>
<tr>
<td colspan="6"><button onclick="setCmdMode(1); moveCmd(setTransto, setHdgto, setSpdto);">Send<br>Command</button></td>
</tr>
<tr><td colspan="6"><hr></td></tr>
</table>
</td>
<div class="section_container">
<div id="bleh" class="sensor_data">
<h2>Sensor Data</h2>
<ul>
<!-- Telemetry Section -->
<li>
<div class="section_container">
<label>Battery Voltage</label>
</div>
<div class="section_container">
<meter id="btry_meter" min="4.0" max="6.0" low="4.5" optimum="5.0" high="4.8" value="5.8"
title="V"></meter>
</div>
</li>
<td style="vertical-align: top;">
<table style="width:100%; border-spacing: 5px; ">
<tr><td colspan="2"><hr></td></tr>
<tr>
<td style="width:50%;"><label>Rover Status</label></td>
<td style="width:50%;"><strong id="Rov_status">X</strong>
</tr>
<tr><td colspan="2"><hr></td></tr>
<tr>
<td style="width:50%;"><label>Signal Strength</label></td>
<td style="width:50%;"><meter id="SigStr" min="-80" max="-30" low="-70" high="-50" optimum="-40" value="-XX"
title="dB"></meter></td>
</tr>
<tr><td colspan="2"><hr></td></tr>
<tr>
<td style="width:50%;"><label>Position</label></td>
<td style="width:50%;"><strong id="PosX">X</strong><strong>,</strong><strong id="PosY">Y</strong></td>
</tr>
<tr>
<td style="width:50%;"><label>Heading</label></td>
<td style="width:50%;"><strong id="Hdg">H</strong><strong>&#176;</strong></td>
</tr>
<tr>
<td style="width:50%;"><label>Trip Distance</label></td>
<td style="width:50%;"><strong id="TrpDist">X</strong><strong> mm</strong></td>
</tr>
<tr><td colspan="2"><hr></td></tr>
<tr>
<td style="width:50%;"><label>Battery Voltage</label></td>
<td style="width:50%;"><strong id="btry_volt">X</strong><strong>V</strong></td>
</tr>
<tr>
<td style="width:50%;"><label>Battery Level</label></td>
<td style="width:50%;"><strong id="btry_lvl">X</strong><strong>%</strong></td>
</tr>
<tr>
<td style="width:50%;"><label>Battery Cycles</label></td>
<td style="width:50%;"><strong id="btry_cycls">X</strong></td>
</tr>
<tr><td colspan="2"><hr></td></tr>
</table>
</td>
<li>
<div class="section_container">
<label>Odometer</label>
</div>
<div class="section_container">
<strong id="Odometer">28</strong><strong>mm</strong>
</div>
</li>
<!-- Command Buffer Section -->
<td style="vertical-align: top;">
<table style="width:100%; border-spacing: 5px; ">
<tr><td colspan="2"><hr></td></tr>
<tr><td colspan="2"><div id="command_space"><p id="command_buffer" style="padding-left: 5px; font-size: 12px; font-family: 'Courier New', Courier, monospace;"></p></div></td></tr>
<tr>
<td><input type="text" placeholder="Type a command or 'help'." id="commandInput" value="" onkeyup="if(event.keyCode == 13){document.getElementById('commandEnter').click();}" style="width: 100%; font-size: 14px; font-family: 'Courier New', Courier, monospace;"></td>
<td style="width: 7%; "><button onclick="setCmdMode(0); RunParser();" id="commandEnter" style="height: 20px; width: 20px; padding: 3px 2px;">&crarr;</button></td>
</tr>
</table>
</ul>
</div>
</div>
</td>
</tr>
</table>
</div>
</body>
<script src="command.js"></script>
<script>
var connection = new WebSocket('ws://' + location.hostname + ':81/');
var command_id = 1;
var mode = 0;
var reqHeading = 0;
var reqDistance = 0;
var reqSpeed = 0;
var reqCharge = 0;
var pstop_time = 0;
var state = 0;
var batteryVoltage = 0;
var batteryLevel = 0;
var totalTripDistance = 0;
var currentHeading = 0;
var current_pos = [,];
var current_x = 0;
var current_y = 0;
var signal_strength = 0;
var lastCompletedCommand_id = 0;
var batteryCycles = 0;
connection.onmessage = function (event) {
var raw_data = event.data;
console.log(raw_data);
var data = JSON.parse(raw_data);
state = data.st;
batteryLevel = data.bV;
batteryVoltage = data.bV;
batteryCycles = data.bC;
totalTripDistance = data.tD;
currentHeading = data.cH;
current_pos = data.pos;
current_x = current_pos[0];
current_y = current_pos[1];
signal_strength = data.rssi;
lastCompletedCommand_id = data.LCCid;
var rStatus = ""
if(state == -1){
rStatus = "Error";
}else if(state == 0){
rStatus = "Idle";
}else if(state == 1){
rStatus = "Moving";
}else if(state == 2){
rStatus = "Charging";
}else{
rStatus = "Undefined";
}
document.getElementById("Rov_status").innerHTML = rStatus;
document.getElementById("SigStr").value = signal_strength;
document.getElementById("PosX").innerHTML = current_x;
document.getElementById("PosY").innerHTML = current_y;
document.getElementById("Hdg").innerHTML = currentHeading;
document.getElementById("TrpDist").innerHTML = totalTripDistance;
document.getElementById("btry_volt").innerHTML = batteryVoltage;
document.getElementById("btry_lvl").innerHTML = batteryLevel;
document.getElementById("btry_cycls").innerHTML = batteryCycles;
}
function send_data() {
var raw_data = '{"Cid":' + command_id + ',"mode":' + mode + ',"rH":' + reqHeading + ',"rD":' + reqDistance + ',"rS":' + reqSpeed + ',"rC":' + reqCharge + ',"pSt":' + pstop_time + '}';
connection.send(raw_data);
console.log(raw_data);
}
function setCmdMode(mode){
cmdMode = mode;
}
function updateCommandBuffer(){
var bufferOutput = "";
if(cmdMode == 0){
bufferOutput = document.getElementById("commandInput").value;
}else if(cmdMode == 1){
bufferOutput = "move " + reqDistance + "mm " + reqHeading + "deg " + reqSpeed + "%";
}
if((mode == 1) || (mode == -1) || (mode == 2) || (mode == 3)){
document.getElementById("command_buffer").innerHTML += '[' + command_id + '] ' + bufferOutput + "<br>";
document.getElementById("commandInput").value = ""
}else if(mode == 0){
document.getElementById("command_buffer").innerHTML = "Rover Emergency Stop." + "<br>" +"Command buffer cleared." + "<br>";
document.getElementById("commandInput").value = "";
}
}
function printHelpDetails(){
document.getElementById("command_buffer").innerHTML =
("------------------------------------------" + "<br>" + "<br>" +
"Types of commands available:" + "<br>" + "<br>" +
"'move' moves rover along a given vector" + "<br>" +
"> move [distance]mm [heading]deg [speed]%" + "<br>" + "<br>" +
"'pstop' short for planned stop it stops the rover without reseting the command buffer" + "<br>" +
"> pstop" + "<br>" + "<br>" +
"'charge' stops the rover and recharges to a set battery level" + "<br>" +
"> charge to [percentage]%" + "<br>" + "<br>" +
"'telemetry reset' resets X,Y coordinates and Trip Distance" + "<br>" +
"> telemetry reset" + "<br>" + "<br>" +
"'stop' and emergency stop that stops the rover and resets the command buffer" + "<br>" +
"> stop" + "<br>" + "<br>" +
"------------------------------------------"
)
document.getElementById("commandInput").value = "";
}
function moveCmd(dist,hdg,spd){
mode = 1;
reqDistance = dist;
reqHeading = hdg;
reqSpeed = spd;
reqCharge = 0;
pstop_time = 0;
tel_rst = 0;
send_data();
updateCommandBuffer();
command_id++;
}
function stpCmd(){
mode = 0;
reqDistance = 0;
reqHeading = 0;
reqSpeed = 0;
reqCharge = 0;
pstop_time = 0;
tel_rst = 0;
send_data();
command_id = 1;
updateCommandBuffer();
}
function pstpCmd(pstp_tme){
mode = 3;
reqDistance = 0;
reqHeading = 0;
reqSpeed = 0;
reqCharge = 0;
pstop_time = pstp_tme;
tel_rst = 0;
send_data();
updateCommandBuffer();
command_id++;
}
function chrgCmd(chrglvl){
mode = 2;
reqDistance = 0;
reqHeading = 0;
reqSpeed = 0;
reqCharge = chrglvl;
pstop_time = 0;
tel_rst = 0;
send_data();
updateCommandBuffer();
command_id++;
}
function telRst(){
mode = -1;
reqDistance = 0;
reqHeading = 0;
reqSpeed = 0;
reqCharge = 0;
pstop_time = 0;
send_data();
updateCommandBuffer();
command_id++;
}
function RunParser(){
var commandstring = document.getElementById("commandInput").value;
try{
command.parse(commandstring);
} catch(err){
alert(err);
}
}
var setHdgto = 270;
var hdg_slider = document.getElementById("HdgSlider");
var hdg_output = document.getElementById("SetHeading");
hdg_output.innerHTML = hdg_slider.value;
hdg_slider.oninput = function() {
hdg_output.innerHTML = this.value;
setHdgto = this.value;
}
var setTransto = 100;
var tran_slider = document.getElementById("TranSlider");
var tran_output = document.getElementById("SetTrans");
tran_output.innerHTML = tran_slider.value;
tran_slider.oninput = function() {
tran_output.innerHTML = this.value;
setTransto = this.value;
}
var setSpdto = 50;
var spd_slider = document.getElementById("SpdSlider");
var spd_output = document.getElementById("SetSpd");
spd_output.innerHTML = spd_slider.value;
spd_slider.oninput = function() {
spd_output.innerHTML = this.value;
setSpdto = this.value;
}
</script>
</html>

View file

@ -13,7 +13,6 @@ platform = espressif32
board = esp32dev
framework = arduino
monitor_speed = 115200
upload_port = COM[3]
monitor_filters =
send_on_enter
esp32_exception_decoder

View file

@ -19,12 +19,12 @@ void loop()
deserializeJson(rdoc, Serial1); // Take JSON input from UART1
int requiredHeading = rdoc["rH"]; // if -1: command in progress, returning requested heading, dist/sp to be ignored
int distance = rdoc["dist"]; // -1 for emergency stop
float speed = rdoc["sp"]; // -1 for emergency stop
float spd = rdoc["sp"]; // -1 for emergency stop
int currentHeading = rdoc["cH"];
bool resetDistanceTravelled = rdoc["rstD"];
bool resetDistanceTravelled = rdoc["rstD"];
bool commandComplete = 0;
float powerUsage_mW = 0.0;
float powerUsage_mWh = 0.0;
int distTravelled_mm = 0;
int current_x = 0;
int current_y = 0;
@ -33,7 +33,7 @@ void loop()
DynamicJsonDocument tdoc(1024); // transmit doc, not sure how big this needs to be
tdoc["comp"] = commandComplete; // If 0: command in progress, current heading requested
tdoc["mW"] = powerUsage_mW;
tdoc["mWh"] = powerUsage_mWh;
tdoc["mm"] = distTravelled_mm;
tdoc["pos"][0] = current_x;
tdoc["pos"][1] = current_y;

59
Control/src/command.jison Normal file
View file

@ -0,0 +1,59 @@
%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'
/lex
%start command
%%
command
: expr EOF
;
expr
: move whitespace distance whitespace heading_angle whitespace percentage
{
var inDist = Number(String($3).substr(0, ((String($3).length) - 2)));
var inHdg = Number(String($5).substr(0, ((String($5).length) - 3)));
var inSpd = Number(String($7).substr(0, ((String($7).length) - 1)));
moveCmd(inDist,inHdg,inSpd);
}
| stop
{
stpCmd();
}
| pstop whitespace stop_duration
{
var inStpDur = Number(String($3).substr(0, ((String($3).length) - 1)));
pstpCmd(inStpDur);
}
| charge_to whitespace percentage
{
var inChrg = Number(String($3).substr(0, ((String($3).length) - 1)));
chrgCmd(inChrg);
}
| telemetry_reset
{
telRst();
}
| help
{
printHelpDetails();
}
;

View file

@ -1,6 +1,7 @@
#pragma region Includes
#include <Arduino.h>
#include <string>
#include <SoftwareSerial.h> // Software Serial not currently needed
#include <SoftwareSerial.h>
#include <AsyncTCP.h>
#include <ESPAsyncWebServer.h>
@ -22,12 +23,14 @@
#pragma endregion
#pragma region Definitions eg pins
#define RX1pin 18 // Pin 6 on expansion board, UART1
#define TX1pin 5 // Pin 7 on expansion board, UART1
#define RX2pin 17 // Pin 8 on expansion board, UART2
#define TX2pin 16 // Pin 9 on expansion board, UART2
#define RX1pin 17 // Pin 6 on expansion board, UART1
#define TX1pin 16 // Pin 7 on expansion board, UART1
#define RX2pin 18 // Pin 8 on expansion board, UART2
#define TX2pin 5 // Pin 9 on expansion board, UART2
#define RX3pin 14 // Pin 10 on expansion board, UART3
#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
#pragma endregion
#pragma region Function Declarations
@ -41,6 +44,7 @@ void sendToEnergy(bool instruction);
void recvFromEnergy();
void sendToVision();
void recvFromVision();
void recvFromCompass();
void emergencyStop();
#pragma endregion
@ -48,7 +52,7 @@ void emergencyStop();
AsyncWebServer webserver(80);
WebSocketsServer websocketserver(81);
Ticker ticker;
SoftwareSerial Serial3;
SoftwareSerial Serial3, Serial4;
std::queue<RoverInstruction> InstrQueue;
#pragma endregion
@ -75,10 +79,11 @@ void setup()
esp_log_level_set("wifi", ESP_LOG_WARN); // enable WARN logs from WiFi stack
esp_log_level_set("dhcpc", ESP_LOG_INFO); // enable INFO logs from DHCP client
Serial.begin(115200); // Set up hardware UART0 (Connected to USB port)
Serial1.begin(9600, SERIAL_8N1, RX1pin, TX1pin); // Set up hardware UART1 (Connected to Drive)
Serial2.begin(9600, SERIAL_8N1, RX2pin, TX2pin); // Set up hardware UART2 (Connected to Energy)
Serial3.begin(9600, SWSERIAL_8N1, RX3pin, TX3pin); // Set up software UART3 (Connected to Vision)
Serial.begin(115200); // Set up hardware UART0 (Connected to USB port)
Serial1.begin(9600, SERIAL_8N1, RX1pin, TX1pin); // Set up hardware UART1 (Connected to Drive)
Serial2.begin(9600, SERIAL_8N1, RX2pin, TX2pin); // Set up hardware UART2 (Connected to Energy)
Serial3.begin(9600, SWSERIAL_8N1, RX3pin, TX3pin); // Set up software UART3 (Connected to Vision)
Serial4.begin(9600, SWSERIAL_8N1, RX4pin, TX4pin); // Set up software UART4 (Connected to Compass)
// Set global variable startup values
Status = CS_IDLE;
@ -108,15 +113,17 @@ void setup()
{
delay(500);
}
while (!MDNS.begin("rover")) // Set up mDNS cast at "rover.local/"
while (!MDNS.begin("rover2")) // Set up mDNS cast at "rover.local/"
{
Serial.println("Error setting up mDNS, retrying in 5s");
delay(5000);
}
Serial.println("mDNS set up, access Control Panel at 'rover.local/'");
Serial.println("mDNS set up, access Control Panel at 'rover2.local/'");
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)
{ request->send(SPIFFS, "/command.js", "text/js"); }); // Serve "command.js" for root page to accessj
webserver.on("/favicon.ico", HTTP_GET, [](AsyncWebServerRequest *request)
{ request->send(SPIFFS, "/favicon.ico", "image/png"); }); // Serve tab icon
webserver.onNotFound(notFound); // Set up basic 404NotFound page
@ -132,7 +139,8 @@ void loop()
websocketserver.loop(); // Handle incoming client connections
recvFromDrive(); // Update stats from Drive
recvFromEnergy(); // Update stats from Energy
recvFromVision(); // Update stats from Vision
// recvFromVision(); // Update stats from Vision
recvFromCompass(); // Update stats from Compass
switch (Status)
{
case CS_ERROR:
@ -195,6 +203,7 @@ void loop()
break;
}
lastExecutedCommand = instr->id; // Update tracker of last processed command
InstrQueue.pop();
}
}
break;
@ -242,7 +251,7 @@ void loop()
}
break;
}
delay(500);
// delay(500);
}
void notFound(AsyncWebServerRequest *request)
@ -447,13 +456,20 @@ void recvFromVision() // Update bounding box and obstacle detection data from Vi
}
}
void recvFromCompass()
{
if (Serial4.available())
{
DynamicJsonDocument rdoc(1024);
deserializeJson(rdoc, Serial4);
heading = rdoc["cH"];
}
}
void emergencyStop()
{
DynamicJsonDocument tdoc(1024);
tdoc["rH"] = heading;
tdoc["dist"] = -1;
tdoc["sp"] = -1;
tdoc["cH"] = heading;
tdoc["stp"] = 1;
serializeJson(tdoc, Serial1); // Send stop signals to Drive
sendToEnergy(0); // Send stop signal to Energy
while (InstrQueue.size())

5
Drive/.gitignore vendored Normal file
View file

@ -0,0 +1,5 @@
.pio
.vscode/.browse.c_cpp.db*
.vscode/c_cpp_properties.json
.vscode/launch.json
.vscode/ipch

7
Drive/.vscode/extensions.json vendored Normal file
View file

@ -0,0 +1,7 @@
{
// See http://go.microsoft.com/fwlink/?LinkId=827846
// for the documentation about the extensions.json format
"recommendations": [
"platformio.platformio-ide"
]
}

39
Drive/include/README Normal file
View file

@ -0,0 +1,39 @@
This directory is intended for project header files.
A header file is a file containing C declarations and macro definitions
to be shared between several project source files. You request the use of a
header file in your project source file (C, C++, etc) located in `src` folder
by including it, with the C preprocessing directive `#include'.
```src/main.c
#include "header.h"
int main (void)
{
...
}
```
Including a header file produces the same results as copying the header file
into each source file that needs it. Such copying would be time-consuming
and error-prone. With a header file, the related declarations appear
in only one place. If they need to be changed, they can be changed in one
place, and programs that include the header file will automatically use the
new version when next recompiled. The header file eliminates the labor of
finding and changing all the copies as well as the risk that a failure to
find one copy will result in inconsistencies within a program.
In C, the usual convention is to give header files names that end with `.h'.
It is most portable to use only letters, digits, dashes, and underscores in
header file names, and at most one dot.
Read more about using header files in official GCC documentation:
* Include Syntax
* Include Operation
* Once-Only Headers
* Computed Includes
https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html

46
Drive/lib/README Normal file
View file

@ -0,0 +1,46 @@
This directory is intended for project specific (private) libraries.
PlatformIO will compile them to static libraries and link into executable file.
The source code of each library should be placed in a an own separate directory
("lib/your_library_name/[here are source files]").
For example, see a structure of the following two libraries `Foo` and `Bar`:
|--lib
| |
| |--Bar
| | |--docs
| | |--examples
| | |--src
| | |- Bar.c
| | |- Bar.h
| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html
| |
| |--Foo
| | |- Foo.c
| | |- Foo.h
| |
| |- README --> THIS FILE
|
|- platformio.ini
|--src
|- main.c
and a contents of `src/main.c`:
```
#include <Foo.h>
#include <Bar.h>
int main (void)
{
...
}
```
PlatformIO Library Dependency Finder will find automatically dependent
libraries scanning project source files.
More information about PlatformIO Library Dependency Finder
- https://docs.platformio.org/page/librarymanager/ldf.html

21
Drive/platformio.ini Normal file
View file

@ -0,0 +1,21 @@
; PlatformIO Project Configuration File
;
; Build options: build flags, source filter
; Upload options: custom upload port, speed and extra flags
; Library options: dependencies, extra library storages
; Advanced options: extra scripting
;
; Please visit documentation for the other options and examples
; https://docs.platformio.org/page/projectconf.html
[env]
lib_deps =
bblanchon/ArduinoJson @ ^6.18.0
wollewald/INA219_WE @ ^1.1.6
monitor_speed = 115200
upload_speed = 115200
[env:nano_every]
platform = atmelmegaavr
board = nano_every
framework = arduino

872
Drive/src/main.cpp Normal file
View file

@ -0,0 +1,872 @@
#include <Arduino.h>
#include <ArduinoJson.h>
// #include <string>
#include <Wire.h>
#include <INA219_WE.h>
#include "SPI.h"
#include <SoftwareSerial.h>
// #define RXpin 4 // Define your RX pin here
// #define TXpin 13 // Define your TX pin here
// SoftwareSerial mySerial(RXpin, TXpin);
bool debug = false;
//TO IMPLEMENT
//DONE 2 way serial
//DONE F<>,B<>,S,L<>,R<>,p<0--1023>
//DONE Obtain current and power usage, get voltage from analog pin
//request angle facing
//DONE speed control 0-1
//speed calibration, 0 stop and max speed to match
//distance travveled and x and y at request
//-------------------------------------------------------SMPS & MOTOR CODE START------------------------------------------------------//
INA219_WE ina219; // this is the instantiation of the library for the current sensor
float open_loop, closed_loop; // Duty Cycles
float vpd, vb, vref, iL, dutyref, current_mA; // Measurement Variables
unsigned int sensorValue0, sensorValue1, sensorValue2, sensorValue3; // ADC sample values declaration
float ev = 0, cv = 0, ei = 0, oc = 0; //internal signals
float Ts = 0.0008; //1.25 kHz control frequency. It's better to design the control period as integral multiple of switching period.
float kpv = 0.05024, kiv = 15.78, kdv = 0; // voltage pid.
float u0v, u1v, delta_uv, e0v, e1v, e2v; // u->output; e->error; 0->this time; 1->last time; 2->last last time
float kpi = 0.02512, kii = 39.4, kdi = 0; // current pid.
float u0i, u1i, delta_ui, e0i, e1i, e2i; // Internal values for the current controller
float uv_max = 4, uv_min = 0; //anti-windup limitation
float ui_max = 1, ui_min = 0; //anti-windup limitation
float current_limit = 1.0;
boolean Boost_mode = 0;
boolean CL_mode = 0;
unsigned int loopTrigger;
unsigned int com_count = 0; // a variables to count the interrupts. Used for program debugging.
//************************** Motor Constants **************************//
unsigned long previousMillis = 0; //initializing time counter
const long f_i = 10000; //time to move in forward direction, please calculate the precision and conversion factor
const long r_i = 20000; //time to rotate clockwise
const long b_i = 30000; //time to move backwards
const long l_i = 40000; //time to move anticlockwise
const long s_i = 50000;
int DIRRstate = LOW; //initializing direction states
int DIRLstate = HIGH;
int DIRL = 20; //defining left direction pin
int DIRR = 21; //defining right direction pin
int pwmr = 5; //pin to control right wheel speed using pwm
int pwml = 9; //pin to control left wheel speed using pwm
//*******************************************************************//
//-------------------------------------------------------SMPS & MOTOR CODE END------------------------------------------------------//
//-------------------------------------------------------OPTICAL SENSOR CODE START------------------------------------------------------//
#define PIN_SS 10
#define PIN_MISO 12
#define PIN_MOSI 11
#define PIN_SCK 13
#define PIN_MOUSECAM_RESET 8
#define PIN_MOUSECAM_CS 7
#define ADNS3080_PIXELS_X 30
#define ADNS3080_PIXELS_Y 30
#define ADNS3080_PRODUCT_ID 0x00
#define ADNS3080_REVISION_ID 0x01
#define ADNS3080_MOTION 0x02
#define ADNS3080_DELTA_X 0x03
#define ADNS3080_DELTA_Y 0x04
#define ADNS3080_SQUAL 0x05
#define ADNS3080_PIXEL_SUM 0x06
#define ADNS3080_MAXIMUM_PIXEL 0x07
#define ADNS3080_CONFIGURATION_BITS 0x0a
#define ADNS3080_EXTENDED_CONFIG 0x0b
#define ADNS3080_DATA_OUT_LOWER 0x0c
#define ADNS3080_DATA_OUT_UPPER 0x0d
#define ADNS3080_SHUTTER_LOWER 0x0e
#define ADNS3080_SHUTTER_UPPER 0x0f
#define ADNS3080_FRAME_PERIOD_LOWER 0x10
#define ADNS3080_FRAME_PERIOD_UPPER 0x11
#define ADNS3080_MOTION_CLEAR 0x12
#define ADNS3080_FRAME_CAPTURE 0x13
#define ADNS3080_SROM_ENABLE 0x14
#define ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER 0x19
#define ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER 0x1a
#define ADNS3080_FRAME_PERIOD_MIN_BOUND_LOWER 0x1b
#define ADNS3080_FRAME_PERIOD_MIN_BOUND_UPPER 0x1c
#define ADNS3080_SHUTTER_MAX_BOUND_LOWER 0x1e
#define ADNS3080_SHUTTER_MAX_BOUND_UPPER 0x1e
#define ADNS3080_SROM_ID 0x1f
#define ADNS3080_OBSERVATION 0x3d
#define ADNS3080_INVERSE_PRODUCT_ID 0x3f
#define ADNS3080_PIXEL_BURST 0x40
#define ADNS3080_MOTION_BURST 0x50
#define ADNS3080_SROM_LOAD 0x60
#define ADNS3080_PRODUCT_ID_VAL 0x17
int total_x = 0;
int total_y = 0;
int total_x1 = 0;
int total_y1 = 0;
int x = 0;
int y = 0;
int a = 0;
int b = 0;
int distance_x = 0;
int distance_y = 0;
int dist_to_move_prev_fl = 0;
int dist_to_move_prev_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;
float kpdrive = 0.055;
float kddrive = 4.700;
float kpheading = 0.055;
float kdheading = 4.700;
volatile byte movementflag = 0;
volatile int xydat[2];
// FUNCTION DELCARATIONS //
float pidi(float pid_input);
float pidv(float pid_input);
void pwm_modulate(float pwm_input);
float saturation(float sat_input, float uplim, float lowlim);
void sampling();
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);
int convTwosComp(int b)
{
//Convert from 2's complement
if (b & 0x80)
{
b = -1 * ((b ^ 0xff) + 1);
}
return b;
}
int tdistance = 0;
void mousecam_reset(){
digitalWrite(PIN_MOUSECAM_RESET, HIGH);
delay(1); // reset pulse >10us
digitalWrite(PIN_MOUSECAM_RESET, LOW);
delay(35); // 35ms from reset to functional
}
int mousecam_init(){
pinMode(PIN_MOUSECAM_RESET, OUTPUT);
pinMode(PIN_MOUSECAM_CS, OUTPUT);
digitalWrite(PIN_MOUSECAM_CS, HIGH);
mousecam_reset();
int pid = mousecam_read_reg(ADNS3080_PRODUCT_ID);
if (pid != ADNS3080_PRODUCT_ID_VAL)
return -1;
// turn on sensitive mode
mousecam_write_reg(ADNS3080_CONFIGURATION_BITS, 0x19);
return 0;
}
void mousecam_write_reg(int reg, int val){
digitalWrite(PIN_MOUSECAM_CS, LOW);
SPI.transfer(reg | 0x80);
SPI.transfer(val);
digitalWrite(PIN_MOUSECAM_CS, HIGH);
delayMicroseconds(50);
}
int mousecam_read_reg(int reg){
digitalWrite(PIN_MOUSECAM_CS, LOW);
SPI.transfer(reg);
delayMicroseconds(75);
int ret = SPI.transfer(0xff);
digitalWrite(PIN_MOUSECAM_CS, HIGH);
delayMicroseconds(1);
return ret;
}
struct MD{
byte motion;
char dx, dy;
byte squal;
word shutter;
byte max_pix;
};
void mousecam_read_motion(struct MD *p){
digitalWrite(PIN_MOUSECAM_CS, LOW);
SPI.transfer(ADNS3080_MOTION_BURST);
delayMicroseconds(75);
p->motion = SPI.transfer(0xff);
p->dx = SPI.transfer(0xff);
p->dy = SPI.transfer(0xff);
p->squal = SPI.transfer(0xff);
p->shutter = SPI.transfer(0xff) << 8;
p->shutter |= SPI.transfer(0xff);
p->max_pix = SPI.transfer(0xff);
digitalWrite(PIN_MOUSECAM_CS, HIGH);
delayMicroseconds(5);
}
// pdata must point to an array of size ADNS3080_PIXELS_X x ADNS3080_PIXELS_Y
// you must call mousecam_reset() after this if you want to go back to normal operation
int mousecam_frame_capture(byte *pdata)
{
mousecam_write_reg(ADNS3080_FRAME_CAPTURE, 0x83);
digitalWrite(PIN_MOUSECAM_CS, LOW);
SPI.transfer(ADNS3080_PIXEL_BURST);
delayMicroseconds(50);
int pix;
byte started = 0;
int count;
int timeout = 0;
int ret = 0;
for (count = 0; count < ADNS3080_PIXELS_X * ADNS3080_PIXELS_Y;)
{
pix = SPI.transfer(0xff);
delayMicroseconds(10);
if (started == 0)
{
if (pix & 0x40)
started = 1;
else
{
timeout++;
if (timeout == 100)
{
ret = -1;
break;
}
}
}
if (started == 1)
{
pdata[count++] = (pix & 0x3f) << 2; // scale to normal grayscale byte range
}
}
digitalWrite(PIN_MOUSECAM_CS, HIGH);
delayMicroseconds(14);
return ret;
}
//-------------------------------------------------------OPTICAL SENSOR CODE END------------------------------------------------------//
//Tracker Variables
int current_x = 0;
int current_y = 0;
int goal_x = 0;
int goal_y = 0;
int distanceGoal;
bool commandComplete = 1;
float powerUsage_mWh = 0;
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
int rightStop = 255; //pwm max for right motor
//Energy Usage Variables
unsigned long previousMillis_Energy = 0; // will store last time energy use was updated
const long interval_Energy = 1000; //energy usaged update frequency
float totalEnergyUsed = 0;
float powerUsed = 0;
int loopCount = 0;
float motorVoltage = 0;
int getPWMfromSpeed(float speedr, bool left)
{
if (speedr >= 1)
{
return 512;
}
else if (speedr < 0)
{
return 0;
}
else
{
int speedpercentage = (speedr * 100);
if (left)
{
return map(speedpercentage, 0, 100, leftStart, leftStop);
}
else
{
return map(speedpercentage, 0, 100, rightStart, rightStop);
}
}
}
void setup()
{
//-------------------------------------------------------SMPS & MOTOR CODE START------------------------------------------------------//
//************************** Motor Pins Defining **************************//
pinMode(DIRR, OUTPUT);
pinMode(DIRL, OUTPUT);
pinMode(pwmr, OUTPUT);
pinMode(pwml, OUTPUT);
digitalWrite(pwmr, HIGH); //setting right motor speed at maximum
digitalWrite(pwml, HIGH); //setting left motor speed at maximum
//*******************************************************************//
//Basic pin setups
noInterrupts(); //disable all interrupts
pinMode(13, OUTPUT); //Pin13 is used to time the loops of the controller
pinMode(3, INPUT_PULLUP); //Pin3 is the input from the Buck/Boost switch
pinMode(2, INPUT_PULLUP); // Pin 2 is the input from the CL/OL switch
analogReference(EXTERNAL); // We are using an external analogue reference for the ADC
// TimerA0 initialization for control-loop interrupt.
TCA0.SINGLE.PER = 999; //
TCA0.SINGLE.CMP1 = 999; //
TCA0.SINGLE.CTRLA = TCA_SINGLE_CLKSEL_DIV16_gc | TCA_SINGLE_ENABLE_bm; //16 prescaler, 1M.
TCA0.SINGLE.INTCTRL = TCA_SINGLE_CMP1_bm;
// TimerB0 initialization for PWM output
pinMode(6, OUTPUT);
TCB0.CTRLA = TCB_CLKSEL_CLKDIV1_gc | TCB_ENABLE_bm; //62.5kHz
analogWrite(6, 120);
interrupts(); //enable interrupts.
Wire.begin(); // We need this for the i2c comms for the current sensor
ina219.init(); // this initiates the current sensor
Wire.setClock(700000); // set the comms speed for i2c
//-------------------------------------------------------SMPS & MOTOR CODE END------------------------------------------------------//
Serial.begin(115200); // Set up hardware UART0 (Connected to USB port)
Serial1.begin(9600); // Set up hardware UART
//Serial.println(getPWMfromSpeed(-1));
//Serial.println(getPWMfromSpeed(256));
//Serial.println(getPWMfromSpeed(0.5));
// Other Drive setup stuff
/////////currentHeading = REQUEST HEADING HERE;
analogWrite(pwmr, 0);
analogWrite(pwml, 0);
//digitalWrite(DIRR, LOW);
//digitalWrite(DIRL, HIGH);
pinMode(PIN_SS, OUTPUT);
pinMode(PIN_MISO, INPUT);
pinMode(PIN_MOSI, OUTPUT);
pinMode(PIN_SCK, OUTPUT);
SPI.begin();
SPI.setClockDivider(SPI_CLOCK_DIV32);
SPI.setDataMode(SPI_MODE3);
SPI.setBitOrder(MSBFIRST);
if (mousecam_init() == -1)
{
Serial.println("Mouse cam failed to init");
while (1)
;
}
}
int commandCompletionStatus = 0; //0-No Command, 1-New Command, 2-Command being run, 3-Command Complete
int requiredHeading = 0;
int distance = 0;
float spd = 0;
int currentHeading = 0;
//reset variables for update on completion
unsigned long previousMillis_Command = 0;
const long interval_Command = 1000;
DeserializationError error;
char asciiart(int k)
{
static char foo[] = "WX86*3I>!;~:,`. ";
return foo[k >> 4];
}
byte frame[ADNS3080_PIXELS_X * ADNS3080_PIXELS_Y];
DynamicJsonDocument rdoc(1024);
void loop()
{
if (Serial1.available() && (commandCompletionStatus == 0)){
// receive doc, not sure how big this needs to be
error = deserializeJson(rdoc, Serial1);
Serial.println("Got serial");
// Test if parsing succeeds.
if (error)
{
//Serial.print(F("deserializeJson() failed: "));
//Serial.println(error.f_str());
return;
}
else
{
//parsing success, prepare command and pull request information
commandCompletionStatus = 1;
requiredHeading = rdoc["rH"];
distance = rdoc["dist"];
spd = rdoc["sp"];
currentHeading = rdoc["cH"];
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;
}
}
//if(current_x!=goal_x)
// Do Drive stuff, set the 5 values above
if (commandCompletionStatus == 0)
{ //noCommand
//Do Nothing just wait
//Serial.println("status0");
}
if (commandCompletionStatus == 1)
{ Serial.println("status1");
//newCommand
//set goals
goal_x += distance * sin(requiredHeading);
goal_y += distance * cos(requiredHeading);
total_y = 0;
total_x = 0;
commandCompletionStatus = 2;
initialAngleSet = false;
}
if (commandCompletionStatus == 2)
{ //Serial.println("status2");
//ongoingCommand
//start moving towards goal
//set angle first
if (!initialAngleSet)
{
//turn to angle
if (currentHeading < requiredHeading)
{ //turn right
Serial.println("turning right");
analogWrite(pwmr, getPWMfromSpeed(spd, false));
analogWrite(pwml, getPWMfromSpeed(spd, 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));
digitalWrite(DIRR, HIGH);
digitalWrite(DIRL, HIGH);
}
else
{
//heading correct therefore move to next step...
//STOP!!!!
digitalWrite(pwmr, LOW);
digitalWrite(pwml, LOW);
initialAngleSet = true;
}
}
else
{ //then move forwards but check angle for drift using optical flow
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);
analogWrite(pwmr, getPWMfromSpeed(speed_r, false));
analogWrite(pwml, getPWMfromSpeed(speed_l, true));
digitalWrite(DIRR, LOW);
digitalWrite(DIRL, HIGH);
}
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);
analogWrite(pwmr, getPWMfromSpeed(speed_r, false));
analogWrite(pwml, getPWMfromSpeed(speed_l, true));
digitalWrite(DIRR, HIGH);
digitalWrite(DIRL, LOW);
}
else if ((total_y == distance))
{ //distance met
//STOP!!!!!
digitalWrite(pwmr, LOW);
digitalWrite(pwml, LOW);
commandCompletionStatus = 3;
initialAngleSet = true;
}
}
}
if (commandCompletionStatus == 3)
{ Serial.println("status3");
//currentPosMatchesOrExceedsRequest
///finish moving
//send update via UART
//prepare feedback variables
commandComplete = true;
current_x = goal_x;
current_y = goal_y;
distTravelled_mm += distance;
//compile energy use
unsigned long currentMillis_Energy = millis();
totalEnergyUsed += (currentMillis_Energy - previousMillis_Energy) * (powerUsed / loopCount) / 1000 / (60 * 60);
previousMillis_Energy = currentMillis_Energy;
if (debug){
Serial.print(motorVoltage);
Serial.print("Energy Used: ");
Serial.print(totalEnergyUsed);
Serial.println("mWh");
}
loopCount = 0; //reset counter to zero
powerUsed = 0; //reset power usage
powerUsage_mWh = totalEnergyUsed;
totalEnergyUsed = 0;
total_x1 = 0;
total_y1 = 0;
DynamicJsonDocument tdoc(1024); // transmit doc, not sure how big this needs to be
tdoc["comp"] = commandComplete;
tdoc["mWh"] = powerUsage_mWh;
tdoc["mm"] = distTravelled_mm;
tdoc["pos"][0] = current_x;
tdoc["pos"][1] = current_y;
serializeJson(tdoc, Serial); // Build JSON and send on UART1
commandCompletionStatus = 0;
}
//Handle power usage
//find motor voltage
//int motorVSensor = analogRead(A5);
//float motorVoltage = motorVSensor * (5.0 / 1023.0);
float motorVoltage = 5;
//find average power
if (current_mA >= 0){
powerUsed += current_mA * motorVoltage;
}
if (debug){
Serial.println(powerUsed);
}
//calculate averages for energy use calculations
loopCount += 1; //handle loop quantity for averaging
//find average current
//find average voltage
//update command/control
if (movementflag){
tdistance = tdistance + convTwosComp(xydat[0]);
// Serial.println("Distance = " + String(tdistance));
movementflag = 0;
delay(3);
}
int val = mousecam_read_reg(ADNS3080_PIXEL_SUM);
MD md;
mousecam_read_motion(&md);
/* for (int i = 0; i < md.squal / 4; i++)
Serial.print('*');
Serial.print(' ');
Serial.print((val * 100) / 351);
Serial.print(' ');
Serial.print(md.shutter);
Serial.print(" (");
Serial.print((int)md.dx);
Serial.print(',');
Serial.print((int)md.dy);
Serial.println(')'); */
// Serial.println(md.max_pix);
delay(100);
distance_x = md.dx; //convTwosComp(md.dx);
distance_y = md.dy; //convTwosComp(md.dy);
total_x1 = (total_x1 + distance_x);
total_y1 = (total_y1 + distance_y);
total_x = 10 * total_x1 / 157; //Conversion from counts per inch to mm (400 counts per inch)
total_y = 10 * total_y1 / 157; //Conversion from counts per inch to mm (400 counts per inch)
/* Serial.print('\n');
Serial.println("Distance_x = " + String(total_x));
Serial.println("Distance_y = " + String(total_y));
Serial.print('\n'); */
//-------------------------------------------------------SMPS & MOTOR CODE START------------------------------------------------------//
unsigned long currentMillis = millis();
if (loopTrigger)
{ // This loop is triggered, it wont run unless there is an interrupt
digitalWrite(13, HIGH); // set pin 13. Pin13 shows the time consumed by each control cycle. It's used for debugging.
// Sample all of the measurements and check which control mode we are in
sampling();
CL_mode = digitalRead(3); // input from the OL_CL switch
Boost_mode = digitalRead(2); // input from the Buck_Boost switch
if (Boost_mode)
{
if (CL_mode)
{ //Closed Loop Boost
pwm_modulate(1); // This disables the Boost as we are not using this mode
}
else
{ // Open Loop Boost
pwm_modulate(1); // This disables the Boost as we are not using this mode
}
}
else
{
if (CL_mode)
{ // Closed Loop Buck
// The closed loop path has a voltage controller cascaded with a current controller. The voltage controller
// creates a current demand based upon the voltage error. This demand is saturated to give current limiting.
// The current loop then gives a duty cycle demand based upon the error between demanded current and measured
// current
current_limit = 3; // Buck has a higher current limit
ev = vref - vb; //voltage error at this time
cv = pidv(ev); //voltage pid
cv = saturation(cv, current_limit, 0); //current demand saturation
ei = cv - iL; //current error
closed_loop = pidi(ei); //current pid
closed_loop = saturation(closed_loop, 0.99, 0.01); //duty_cycle saturation
pwm_modulate(closed_loop); //pwm modulation
}
else
{ // Open Loop Buck
current_limit = 3; // Buck has a higher current limit
oc = iL - current_limit; // Calculate the difference between current measurement and current limit
if (oc > 0)
{
open_loop = open_loop - 0.001; // We are above the current limit so less duty cycle
}
else
{
open_loop = open_loop + 0.001; // We are below the current limit so more duty cycle
}
open_loop = saturation(open_loop, dutyref, 0.02); // saturate the duty cycle at the reference or a min of 0.01
pwm_modulate(open_loop); // and send it out
}
}
// closed loop control path
digitalWrite(13, LOW); // reset pin13.
loopTrigger = 0;
}
}
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.
ISR(TCA0_CMP1_vect){
TCA0.SINGLE.INTFLAGS |= TCA_SINGLE_CMP1_bm; //clear interrupt flag
loopTrigger = 1;
}
// This subroutine processes all of the analogue samples, creating the required values for the main loop
void sampling(){
// Make the initial sampling operations for the circuit measurements
sensorValue0 = analogRead(A0); //sample Vb
sensorValue2 = analogRead(A2); //sample Vref
sensorValue3 = analogRead(A3); //sample Vpd
current_mA = ina219.getCurrent_mA(); // sample the inductor current (via the sensor chip)
// Process the values so they are a bit more usable/readable
// 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
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
// differently from the Vref, this time scaled between zero and 1.
// The boost duty cycle needs to be saturated with a 0.33 minimum to prevent high output voltages
if (Boost_mode == 1){
iL = -current_mA / 1000.0;
dutyref = saturation(sensorValue2 * (1.0 / 1023.0), 0.99, 0.33);
}else{
iL = current_mA / 1000.0;
dutyref = sensorValue2 * (1.0 / 1023.0);
}
}
float saturation(float sat_input, float uplim, float lowlim){ // Saturation function
if (sat_input > uplim)
sat_input = uplim;
else if (sat_input < lowlim)
sat_input = lowlim;
else
;
return sat_input;
}
void pwm_modulate(float pwm_input){ // PWM function
analogWrite(6, (int)(255 - pwm_input * 255));
}
// This is a PID controller for the voltage
float pidv(float pid_input){
float e_integration;
e0v = pid_input;
e_integration = e0v;
//anti-windup, if last-time pid output reaches the limitation, this time there won't be any intergrations.
if (u1v >= uv_max){
e_integration = 0;
}else if (u1v <= uv_min){
e_integration = 0;
}
delta_uv = kpv * (e0v - e1v) + kiv * Ts * e_integration + kdv / Ts * (e0v - 2 * e1v + e2v); //incremental PID programming avoids integrations.there is another PID program called positional PID.
u0v = u1v + delta_uv; //this time's control output
//output limitation
saturation(u0v, uv_max, uv_min);
u1v = u0v; //update last time's control output
e2v = e1v; //update last last time's error
e1v = e0v; // update last time's error
return u0v;
}
// This is a PID controller for the current
float pidi(float pid_input){
float e_integration;
e0i = pid_input;
e_integration = e0i;
//anti-windup
if (u1i >= ui_max){
e_integration = 0;
}
else if (u1i <= ui_min){
e_integration = 0;
}
delta_ui = kpi * (e0i - e1i) + kii * Ts * e_integration + kdi / Ts * (e0i - 2 * e1i + e2i); //incremental PID programming avoids integrations.
u0i = u1i + delta_ui; //this time's control output
//output limitation
saturation(u0i, ui_max, ui_min);
u1i = u0i; //update last time's control output
e2i = e1i; //update last last time's error
e1i = e0i; // update last time's error
return u0i;
}
// 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){
int T_diff = millis() - *time_pid_prev;
float speed = (kps * dist_to_move) + ((kds/T_diff) * (dist_to_move - *dist_to_move_prev));
*time_pid_prev = millis();
Serial.println(speed);
if (speed >= 1) speed = 1;
else if (speed <= 0.5) speed = 0.5;
*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){
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;
}
*time_pid_prev = millis();
if (speed_aug >= 1) speed_aug = 1;
else if (speed_aug <= 0.3) speed_aug = 0.3;
*dist_to_move_prev = dist_to_move;
return speed_aug;
}

11
Drive/test/README Normal file
View file

@ -0,0 +1,11 @@
This directory is intended for PlatformIO Unit Testing and project tests.
Unit Testing is a software testing method by which individual units of
source code, sets of one or more MCU program modules together with associated
control data, usage procedures, and operating procedures, are tested to
determine whether they are fit for use. Unit testing finds problems early
in the development cycle.
More information about PlatformIO Unit Testing:
- https://docs.platformio.org/page/plus/unit-testing.html

5
IMU/.gitignore vendored Normal file
View file

@ -0,0 +1,5 @@
.pio
.vscode/.browse.c_cpp.db*
.vscode/c_cpp_properties.json
.vscode/launch.json
.vscode/ipch

7
IMU/.vscode/extensions.json vendored Normal file
View file

@ -0,0 +1,7 @@
{
// See http://go.microsoft.com/fwlink/?LinkId=827846
// for the documentation about the extensions.json format
"recommendations": [
"platformio.platformio-ide"
]
}

39
IMU/include/README Normal file
View file

@ -0,0 +1,39 @@
This directory is intended for project header files.
A header file is a file containing C declarations and macro definitions
to be shared between several project source files. You request the use of a
header file in your project source file (C, C++, etc) located in `src` folder
by including it, with the C preprocessing directive `#include'.
```src/main.c
#include "header.h"
int main (void)
{
...
}
```
Including a header file produces the same results as copying the header file
into each source file that needs it. Such copying would be time-consuming
and error-prone. With a header file, the related declarations appear
in only one place. If they need to be changed, they can be changed in one
place, and programs that include the header file will automatically use the
new version when next recompiled. The header file eliminates the labor of
finding and changing all the copies as well as the risk that a failure to
find one copy will result in inconsistencies within a program.
In C, the usual convention is to give header files names that end with `.h'.
It is most portable to use only letters, digits, dashes, and underscores in
header file names, and at most one dot.
Read more about using header files in official GCC documentation:
* Include Syntax
* Include Operation
* Once-Only Headers
* Computed Includes
https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html

46
IMU/lib/README Normal file
View file

@ -0,0 +1,46 @@
This directory is intended for project specific (private) libraries.
PlatformIO will compile them to static libraries and link into executable file.
The source code of each library should be placed in a an own separate directory
("lib/your_library_name/[here are source files]").
For example, see a structure of the following two libraries `Foo` and `Bar`:
|--lib
| |
| |--Bar
| | |--docs
| | |--examples
| | |--src
| | |- Bar.c
| | |- Bar.h
| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html
| |
| |--Foo
| | |- Foo.c
| | |- Foo.h
| |
| |- README --> THIS FILE
|
|- platformio.ini
|--src
|- main.c
and a contents of `src/main.c`:
```
#include <Foo.h>
#include <Bar.h>
int main (void)
{
...
}
```
PlatformIO Library Dependency Finder will find automatically dependent
libraries scanning project source files.
More information about PlatformIO Library Dependency Finder
- https://docs.platformio.org/page/librarymanager/ldf.html

21
IMU/platformio.ini Normal file
View file

@ -0,0 +1,21 @@
; PlatformIO Project Configuration File
;
; Build options: build flags, source filter
; Upload options: custom upload port, speed and extra flags
; Library options: dependencies, extra library storages
; Advanced options: extra scripting
;
; Please visit documentation for the other options and examples
; https://docs.platformio.org/page/projectconf.html
[env:m5stick-c]
platform = espressif32
board = m5stick-c
framework = arduino
lib_deps =
tanakamasayuki/I2C MPU6886 IMU@^1.0.0
m5stack/M5StickC@^0.2.0
bblanchon/ArduinoJson@^6.18.0
monitor_speed = 115200
monitor_port = COM8
upload_port = COM8

25
IMU/src/main.cpp Normal file
View file

@ -0,0 +1,25 @@
#include <Arduino.h>
#include <M5StickC.h>
#include <ArduinoJson.h>
float pitch, roll, yaw;
void setup() {
M5.begin();
M5.IMU.Init();
Serial.begin(115200);
Serial1.begin(9600, SERIAL_8N1, 26, 0);
}
void loop() {
M5.IMU.getAhrsData(&pitch, &roll, &yaw);
M5.Lcd.setCursor(0, 45);
M5.Lcd.printf("%5.2f\n", roll);
// Serial.printf("{\"cH\":%5.2f}\n", roll);
// Serial1.printf("{\"cH\":%5.2f}\n", roll);
DynamicJsonDocument tdoc(1024);
tdoc["cH"] = roll;
serializeJson(tdoc, Serial1);
delay(50);
}

11
IMU/test/README Normal file
View file

@ -0,0 +1,11 @@
This directory is intended for PlatformIO Unit Testing and project tests.
Unit Testing is a software testing method by which individual units of
source code, sets of one or more MCU program modules together with associated
control data, usage procedures, and operating procedures, are tested to
determine whether they are fit for use. Unit testing finds problems early
in the development cycle.
More information about PlatformIO Unit Testing:
- https://docs.platformio.org/page/plus/unit-testing.html