mirror of
https://github.com/supleed2/ELEC50003-P1-CW.git
synced 2024-12-22 21:45:49 +00:00
Merge branch 'main' into Testad3919
This commit is contained in:
commit
bbe36fb9dd
656
Control/data/command.js
Normal file
656
Control/data/command.js
Normal 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));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -12,82 +12,91 @@
|
||||||
box-sizing: border-box;
|
box-sizing: border-box;
|
||||||
}
|
}
|
||||||
|
|
||||||
.section_container {
|
|
||||||
float: left;
|
|
||||||
width: 50%;
|
|
||||||
padding: 10px;
|
|
||||||
}
|
|
||||||
|
|
||||||
.flex-container {
|
.flex-container {
|
||||||
display: flex;
|
display: flex;
|
||||||
flex-wrap: nowrap;
|
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) {
|
:is(h1, h2, h3, h4, h5, h6, label, strong, meter) {
|
||||||
font-family: Arial, Helvetica, sans-serif;
|
font-family: Arial, Helvetica, sans-serif;
|
||||||
}
|
}
|
||||||
|
|
||||||
.movement_control {
|
h2{
|
||||||
text-align: center;
|
margin: 0px;
|
||||||
}
|
}
|
||||||
|
|
||||||
.sensor_data {
|
.slider {
|
||||||
text-align: center;
|
-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 {
|
meter {
|
||||||
width: 100%;
|
width: 100%;
|
||||||
height: 40px;
|
height: 40px;
|
||||||
transform: translateY(-8px);
|
transform: translateY(8px);
|
||||||
}
|
}
|
||||||
|
|
||||||
meter::after {
|
meter::after {
|
||||||
content: attr(value) attr(title);
|
content: attr(value) attr(title);
|
||||||
top: -28px;
|
top: -28px;
|
||||||
left: 0px;
|
left: 45%;
|
||||||
position: relative;
|
position: relative;
|
||||||
}
|
}
|
||||||
|
|
||||||
.button {
|
button {
|
||||||
|
width: 100%;
|
||||||
display: inline-block;
|
display: inline-block;
|
||||||
padding: 15px 25px;
|
padding: 7px 7px;
|
||||||
font-size: 24px;
|
font-size: 12px;
|
||||||
cursor: pointer;
|
cursor: pointer;
|
||||||
text-align: center;
|
text-align: center;
|
||||||
text-decoration: none;
|
text-decoration: none;
|
||||||
outline: none;
|
outline: none;
|
||||||
color: rgb(255, 255, 255);
|
color: rgb(255, 255, 255);
|
||||||
background-color: #161616;
|
background-color: #222222;
|
||||||
border: none;
|
border: none;
|
||||||
border-radius: 15px;
|
border-radius: 5px;
|
||||||
box-shadow: 0 9px rgb(161, 161, 161);
|
box-shadow: 0 3px rgb(161, 161, 161);
|
||||||
|
-webkit-transition: .2s;
|
||||||
|
transition: background-color .2s;
|
||||||
}
|
}
|
||||||
|
|
||||||
.button:hover {
|
button:hover {
|
||||||
background-color: #585858
|
background-color: #585858
|
||||||
}
|
}
|
||||||
|
|
||||||
.button:active {
|
button:active {
|
||||||
background-color: #107C10;
|
box-shadow: 0 3px rgb(161, 161, 161);
|
||||||
box-shadow: 0 5px rgb(161, 161, 161);
|
transform: translateY(1px);
|
||||||
transform: translateY(4px);
|
|
||||||
}
|
|
||||||
|
|
||||||
.pressed {
|
|
||||||
background-color: #107C10;
|
|
||||||
box-shadow: 0 5px rgb(161, 161, 161);
|
|
||||||
transform: translateY(4px);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
.clearfix::after {
|
.clearfix::after {
|
||||||
|
|
@ -96,203 +105,337 @@
|
||||||
display: table;
|
display: table;
|
||||||
}
|
}
|
||||||
</style>
|
</style>
|
||||||
<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>
|
||||||
|
|
||||||
|
<!-- 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>
|
||||||
|
|
||||||
|
<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>°</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>
|
||||||
|
|
||||||
|
<!-- Telemetry Section -->
|
||||||
|
|
||||||
|
<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>°</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>
|
||||||
|
|
||||||
|
<!-- 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;">↵</button></td>
|
||||||
|
</tr>
|
||||||
|
</table>
|
||||||
|
|
||||||
|
</td>
|
||||||
|
</tr>
|
||||||
|
</table>
|
||||||
|
|
||||||
|
</div>
|
||||||
|
|
||||||
|
</body>
|
||||||
|
<script src="command.js"></script>
|
||||||
|
<script>
|
||||||
var connection = new WebSocket('ws://' + location.hostname + ':81/');
|
var connection = new WebSocket('ws://' + location.hostname + ':81/');
|
||||||
|
|
||||||
var MVM_F_status = 0;
|
var command_id = 1;
|
||||||
var MVM_L_status = 0;
|
var mode = 0;
|
||||||
var MVM_R_status = 0;
|
var reqHeading = 0;
|
||||||
var MVM_B_status = 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;
|
||||||
|
|
||||||
var BTRY_VOLT = 0;
|
|
||||||
var ODO_DIST = 0;
|
|
||||||
|
|
||||||
connection.onmessage = function (event) {
|
connection.onmessage = function (event) {
|
||||||
var raw_data = event.data;
|
var raw_data = event.data;
|
||||||
console.log(raw_data);
|
console.log(raw_data);
|
||||||
var data = JSON.parse(raw_data);
|
var data = JSON.parse(raw_data);
|
||||||
BTRY_VOLT = data.BTRY_VOLT;
|
|
||||||
ODO_DIST = data.ODO_DIST;
|
state = data.st;
|
||||||
document.getElementById("btry_meter").value = BTRY_VOLT;
|
batteryLevel = data.bV;
|
||||||
document.getElementById("Odometer").innerHTML = ODO_DIST;
|
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() {
|
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 + '}';
|
var raw_data = '{"Cid":' + command_id + ',"mode":' + mode + ',"rH":' + reqHeading + ',"rD":' + reqDistance + ',"rS":' + reqSpeed + ',"rC":' + reqCharge + ',"pSt":' + pstop_time + '}';
|
||||||
connection.send(raw_data);
|
connection.send(raw_data);
|
||||||
console.log(raw_data);
|
console.log(raw_data);
|
||||||
}
|
}
|
||||||
|
|
||||||
function left_pressed() {
|
function setCmdMode(mode){
|
||||||
MVM_L_status = 1;
|
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();
|
send_data();
|
||||||
|
updateCommandBuffer();
|
||||||
|
command_id++;
|
||||||
}
|
}
|
||||||
function left_unpressed() {
|
|
||||||
MVM_L_status = 0;
|
function stpCmd(){
|
||||||
|
mode = 0;
|
||||||
|
reqDistance = 0;
|
||||||
|
reqHeading = 0;
|
||||||
|
reqSpeed = 0;
|
||||||
|
reqCharge = 0;
|
||||||
|
pstop_time = 0;
|
||||||
|
tel_rst = 0;
|
||||||
send_data();
|
send_data();
|
||||||
|
command_id = 1;
|
||||||
|
updateCommandBuffer();
|
||||||
}
|
}
|
||||||
function up_pressed() {
|
|
||||||
MVM_F_status = 1;
|
function pstpCmd(pstp_tme){
|
||||||
|
mode = 3;
|
||||||
|
reqDistance = 0;
|
||||||
|
reqHeading = 0;
|
||||||
|
reqSpeed = 0;
|
||||||
|
reqCharge = 0;
|
||||||
|
pstop_time = pstp_tme;
|
||||||
|
tel_rst = 0;
|
||||||
send_data();
|
send_data();
|
||||||
|
updateCommandBuffer();
|
||||||
|
command_id++;
|
||||||
}
|
}
|
||||||
function up_unpressed() {
|
|
||||||
MVM_F_status = 0;
|
function chrgCmd(chrglvl){
|
||||||
|
mode = 2;
|
||||||
|
reqDistance = 0;
|
||||||
|
reqHeading = 0;
|
||||||
|
reqSpeed = 0;
|
||||||
|
reqCharge = chrglvl;
|
||||||
|
pstop_time = 0;
|
||||||
|
tel_rst = 0;
|
||||||
send_data();
|
send_data();
|
||||||
|
updateCommandBuffer();
|
||||||
|
command_id++;
|
||||||
}
|
}
|
||||||
function right_pressed() {
|
|
||||||
MVM_R_status = 1;
|
function telRst(){
|
||||||
|
mode = -1;
|
||||||
|
reqDistance = 0;
|
||||||
|
reqHeading = 0;
|
||||||
|
reqSpeed = 0;
|
||||||
|
reqCharge = 0;
|
||||||
|
pstop_time = 0;
|
||||||
send_data();
|
send_data();
|
||||||
}
|
updateCommandBuffer();
|
||||||
function right_unpressed() {
|
command_id++;
|
||||||
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) {
|
function RunParser(){
|
||||||
switch (e.keyCode) {
|
var commandstring = document.getElementById("commandInput").value;
|
||||||
case 37:
|
try{
|
||||||
document.getElementById("left_arrow").className = "button pressed";
|
command.parse(commandstring);
|
||||||
left_pressed();
|
} catch(err){
|
||||||
break;
|
alert(err);
|
||||||
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>
|
var setHdgto = 270;
|
||||||
</head>
|
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;
|
||||||
|
}
|
||||||
|
|
||||||
<body>
|
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;
|
||||||
|
}
|
||||||
|
|
||||||
<h1 style="text-align:center;">ROVER COMMAND CENTER</h1>
|
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;
|
||||||
|
}
|
||||||
|
|
||||||
<div class="clearfix">
|
</script>
|
||||||
|
|
||||||
<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>⇧</span></button>
|
|
||||||
</div>
|
|
||||||
<div style="transform: translateY(13px);">
|
|
||||||
<button id="left_arrow" onmousedown="left_mouseDown()" onmouseup="left_mouseUp()"
|
|
||||||
class="button"><span>⇦</span></button>
|
|
||||||
<button id="down_arrow" onmousedown="down_mouseDown()" onmouseup="down_mouseUp()"
|
|
||||||
class="button"><span>⇩</span></button>
|
|
||||||
<button id="right_arrow" onmousedown="right_mouseDown()" onmouseup="right_mouseUp()"
|
|
||||||
class="button"><span>⇨</span></button>
|
|
||||||
</div>
|
|
||||||
|
|
||||||
</div>
|
|
||||||
</div>
|
|
||||||
|
|
||||||
<div class="section_container">
|
|
||||||
<div id="bleh" class="sensor_data">
|
|
||||||
<h2>Sensor Data</h2>
|
|
||||||
<ul>
|
|
||||||
|
|
||||||
<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>
|
|
||||||
|
|
||||||
|
|
||||||
<li>
|
|
||||||
<div class="section_container">
|
|
||||||
<label>Odometer</label>
|
|
||||||
</div>
|
|
||||||
<div class="section_container">
|
|
||||||
<strong id="Odometer">28</strong><strong>mm</strong>
|
|
||||||
</div>
|
|
||||||
</li>
|
|
||||||
|
|
||||||
</ul>
|
|
||||||
</div>
|
|
||||||
|
|
||||||
</div>
|
|
||||||
|
|
||||||
</div>
|
|
||||||
|
|
||||||
</body>
|
|
||||||
|
|
||||||
</html>
|
</html>
|
||||||
|
|
@ -13,7 +13,6 @@ platform = espressif32
|
||||||
board = esp32dev
|
board = esp32dev
|
||||||
framework = arduino
|
framework = arduino
|
||||||
monitor_speed = 115200
|
monitor_speed = 115200
|
||||||
upload_port = COM[3]
|
|
||||||
monitor_filters =
|
monitor_filters =
|
||||||
send_on_enter
|
send_on_enter
|
||||||
esp32_exception_decoder
|
esp32_exception_decoder
|
||||||
|
|
|
||||||
|
|
@ -19,12 +19,12 @@ void loop()
|
||||||
deserializeJson(rdoc, Serial1); // Take JSON input from UART1
|
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 requiredHeading = rdoc["rH"]; // if -1: command in progress, returning requested heading, dist/sp to be ignored
|
||||||
int distance = rdoc["dist"]; // -1 for emergency stop
|
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"];
|
int currentHeading = rdoc["cH"];
|
||||||
bool resetDistanceTravelled = rdoc["rstD"];
|
bool resetDistanceTravelled = rdoc["rstD"];
|
||||||
|
|
||||||
bool commandComplete = 0;
|
bool commandComplete = 0;
|
||||||
float powerUsage_mW = 0.0;
|
float powerUsage_mWh = 0.0;
|
||||||
int distTravelled_mm = 0;
|
int distTravelled_mm = 0;
|
||||||
int current_x = 0;
|
int current_x = 0;
|
||||||
int current_y = 0;
|
int current_y = 0;
|
||||||
|
|
@ -33,7 +33,7 @@ void loop()
|
||||||
|
|
||||||
DynamicJsonDocument tdoc(1024); // transmit doc, not sure how big this needs to be
|
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["comp"] = commandComplete; // If 0: command in progress, current heading requested
|
||||||
tdoc["mW"] = powerUsage_mW;
|
tdoc["mWh"] = powerUsage_mWh;
|
||||||
tdoc["mm"] = distTravelled_mm;
|
tdoc["mm"] = distTravelled_mm;
|
||||||
tdoc["pos"][0] = current_x;
|
tdoc["pos"][0] = current_x;
|
||||||
tdoc["pos"][1] = current_y;
|
tdoc["pos"][1] = current_y;
|
||||||
|
|
|
||||||
59
Control/src/command.jison
Normal file
59
Control/src/command.jison
Normal 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();
|
||||||
|
}
|
||||||
|
;
|
||||||
|
|
||||||
|
|
@ -1,6 +1,7 @@
|
||||||
#pragma region Includes
|
#pragma region Includes
|
||||||
#include <Arduino.h>
|
#include <Arduino.h>
|
||||||
#include <string>
|
#include <string>
|
||||||
|
#include <SoftwareSerial.h> // Software Serial not currently needed
|
||||||
#include <SoftwareSerial.h>
|
#include <SoftwareSerial.h>
|
||||||
#include <AsyncTCP.h>
|
#include <AsyncTCP.h>
|
||||||
#include <ESPAsyncWebServer.h>
|
#include <ESPAsyncWebServer.h>
|
||||||
|
|
@ -22,12 +23,14 @@
|
||||||
#pragma endregion
|
#pragma endregion
|
||||||
|
|
||||||
#pragma region Definitions eg pins
|
#pragma region Definitions eg pins
|
||||||
#define RX1pin 18 // Pin 6 on expansion board, UART1
|
#define RX1pin 17 // Pin 6 on expansion board, UART1
|
||||||
#define TX1pin 5 // Pin 7 on expansion board, UART1
|
#define TX1pin 16 // Pin 7 on expansion board, UART1
|
||||||
#define RX2pin 17 // Pin 8 on expansion board, UART2
|
#define RX2pin 18 // Pin 8 on expansion board, UART2
|
||||||
#define TX2pin 16 // Pin 9 on expansion board, UART2
|
#define TX2pin 5 // Pin 9 on expansion board, UART2
|
||||||
#define RX3pin 14 // Pin 10 on expansion board, UART3
|
#define RX3pin 14 // Pin 10 on expansion board, UART3
|
||||||
#define TX3pin 4 // Pin 11 on expansion board, UART3
|
#define TX3pin 4 // Pin 11 on expansion board, UART3
|
||||||
|
#define RX4pin 15 // Pin 12 on expansion board, UART4
|
||||||
|
#define TX4pin 2 // Pin 13 on expansion board, UART4
|
||||||
#pragma endregion
|
#pragma endregion
|
||||||
|
|
||||||
#pragma region Function Declarations
|
#pragma region Function Declarations
|
||||||
|
|
@ -41,6 +44,7 @@ void sendToEnergy(bool instruction);
|
||||||
void recvFromEnergy();
|
void recvFromEnergy();
|
||||||
void sendToVision();
|
void sendToVision();
|
||||||
void recvFromVision();
|
void recvFromVision();
|
||||||
|
void recvFromCompass();
|
||||||
void emergencyStop();
|
void emergencyStop();
|
||||||
#pragma endregion
|
#pragma endregion
|
||||||
|
|
||||||
|
|
@ -48,7 +52,7 @@ void emergencyStop();
|
||||||
AsyncWebServer webserver(80);
|
AsyncWebServer webserver(80);
|
||||||
WebSocketsServer websocketserver(81);
|
WebSocketsServer websocketserver(81);
|
||||||
Ticker ticker;
|
Ticker ticker;
|
||||||
SoftwareSerial Serial3;
|
SoftwareSerial Serial3, Serial4;
|
||||||
std::queue<RoverInstruction> InstrQueue;
|
std::queue<RoverInstruction> InstrQueue;
|
||||||
#pragma endregion
|
#pragma endregion
|
||||||
|
|
||||||
|
|
@ -79,6 +83,7 @@ void setup()
|
||||||
Serial1.begin(9600, SERIAL_8N1, RX1pin, TX1pin); // Set up hardware UART1 (Connected to Drive)
|
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)
|
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)
|
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
|
// Set global variable startup values
|
||||||
Status = CS_IDLE;
|
Status = CS_IDLE;
|
||||||
|
|
@ -108,15 +113,17 @@ void setup()
|
||||||
{
|
{
|
||||||
delay(500);
|
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");
|
Serial.println("Error setting up mDNS, retrying in 5s");
|
||||||
delay(5000);
|
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)
|
webserver.on("/", HTTP_GET, [](AsyncWebServerRequest *request)
|
||||||
{ request->send(SPIFFS, "/index.html", "text/html"); }); // Serve "index.html" at root page
|
{ request->send(SPIFFS, "/index.html", "text/html"); }); // Serve "index.html" at root page
|
||||||
|
webserver.on("/command.js", HTTP_GET, [](AsyncWebServerRequest *request)
|
||||||
|
{ request->send(SPIFFS, "/command.js", "text/js"); }); // Serve "command.js" for root page to accessj
|
||||||
webserver.on("/favicon.ico", HTTP_GET, [](AsyncWebServerRequest *request)
|
webserver.on("/favicon.ico", HTTP_GET, [](AsyncWebServerRequest *request)
|
||||||
{ request->send(SPIFFS, "/favicon.ico", "image/png"); }); // Serve tab icon
|
{ request->send(SPIFFS, "/favicon.ico", "image/png"); }); // Serve tab icon
|
||||||
webserver.onNotFound(notFound); // Set up basic 404NotFound page
|
webserver.onNotFound(notFound); // Set up basic 404NotFound page
|
||||||
|
|
@ -132,7 +139,8 @@ void loop()
|
||||||
websocketserver.loop(); // Handle incoming client connections
|
websocketserver.loop(); // Handle incoming client connections
|
||||||
recvFromDrive(); // Update stats from Drive
|
recvFromDrive(); // Update stats from Drive
|
||||||
recvFromEnergy(); // Update stats from Energy
|
recvFromEnergy(); // Update stats from Energy
|
||||||
recvFromVision(); // Update stats from Vision
|
// recvFromVision(); // Update stats from Vision
|
||||||
|
recvFromCompass(); // Update stats from Compass
|
||||||
switch (Status)
|
switch (Status)
|
||||||
{
|
{
|
||||||
case CS_ERROR:
|
case CS_ERROR:
|
||||||
|
|
@ -195,6 +203,7 @@ void loop()
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
lastExecutedCommand = instr->id; // Update tracker of last processed command
|
lastExecutedCommand = instr->id; // Update tracker of last processed command
|
||||||
|
InstrQueue.pop();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
|
@ -242,7 +251,7 @@ void loop()
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
delay(500);
|
// delay(500);
|
||||||
}
|
}
|
||||||
|
|
||||||
void notFound(AsyncWebServerRequest *request)
|
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()
|
void emergencyStop()
|
||||||
{
|
{
|
||||||
DynamicJsonDocument tdoc(1024);
|
DynamicJsonDocument tdoc(1024);
|
||||||
tdoc["rH"] = heading;
|
tdoc["stp"] = 1;
|
||||||
tdoc["dist"] = -1;
|
|
||||||
tdoc["sp"] = -1;
|
|
||||||
tdoc["cH"] = heading;
|
|
||||||
serializeJson(tdoc, Serial1); // Send stop signals to Drive
|
serializeJson(tdoc, Serial1); // Send stop signals to Drive
|
||||||
sendToEnergy(0); // Send stop signal to Energy
|
sendToEnergy(0); // Send stop signal to Energy
|
||||||
while (InstrQueue.size())
|
while (InstrQueue.size())
|
||||||
|
|
|
||||||
5
Drive/.gitignore
vendored
Normal file
5
Drive/.gitignore
vendored
Normal 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
7
Drive/.vscode/extensions.json
vendored
Normal 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
39
Drive/include/README
Normal 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
46
Drive/lib/README
Normal 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
21
Drive/platformio.ini
Normal 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
872
Drive/src/main.cpp
Normal 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
11
Drive/test/README
Normal 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
5
IMU/.gitignore
vendored
Normal 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
7
IMU/.vscode/extensions.json
vendored
Normal 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
39
IMU/include/README
Normal 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
46
IMU/lib/README
Normal 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
21
IMU/platformio.ini
Normal 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
25
IMU/src/main.cpp
Normal 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
11
IMU/test/README
Normal 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
|
||||||
Loading…
Reference in a new issue