diff --git a/Control/data/command.js b/Control/data/command.js new file mode 100644 index 0000000..091ecc2 --- /dev/null +++ b/Control/data/command.js @@ -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)); +} +} \ No newline at end of file diff --git a/Control/data/index.html b/Control/data/index.html index 152b6f2..12f4278 100644 --- a/Control/data/index.html +++ b/Control/data/index.html @@ -12,82 +12,91 @@ box-sizing: border-box; } - .section_container { - float: left; - width: 50%; - padding: 10px; - } - .flex-container { display: flex; flex-wrap: nowrap; } - ul { - list-style-type: none; - margin: 0; - padding: 0; - } - - li { - padding: 0px; - margin-bottom: 0px; - } - :is(h1, h2, h3, h4, h5, h6, label, strong, meter) { font-family: Arial, Helvetica, sans-serif; } - .movement_control { - text-align: center; + h2{ + margin: 0px; } - .sensor_data { - text-align: center; + .slider { + -webkit-appearance: none; + width: 100%; + height: 25px; + background: #d3d3d3; + outline: none; + opacity: 0.7; + -webkit-transition: .2s; + transition: opacity .2s; + } + + .slider:hover { + opacity: 1; + } + + .slider::-webkit-slider-thumb { + -webkit-appearance: none; + appearance: none; + width: 25px; + height: 25px; + background: #000000; + cursor: pointer; + } + + #command_space { + width:100%; + height: 200px; + line-height: normal; + overflow: auto; + background-color: rgb(248, 248, 248); + color: black; + border: 1px solid black; } meter { width: 100%; height: 40px; - transform: translateY(-8px); + transform: translateY(8px); } meter::after { content: attr(value) attr(title); top: -28px; - left: 0px; + left: 45%; position: relative; } - .button { + button { + width: 100%; display: inline-block; - padding: 15px 25px; - font-size: 24px; + padding: 7px 7px; + font-size: 12px; cursor: pointer; text-align: center; text-decoration: none; outline: none; color: rgb(255, 255, 255); - background-color: #161616; + background-color: #222222; border: none; - border-radius: 15px; - box-shadow: 0 9px rgb(161, 161, 161); + border-radius: 5px; + box-shadow: 0 3px rgb(161, 161, 161); + -webkit-transition: .2s; + transition: background-color .2s; } - .button:hover { + button:hover { background-color: #585858 } - .button:active { - background-color: #107C10; - box-shadow: 0 5px rgb(161, 161, 161); - transform: translateY(4px); - } - - .pressed { - background-color: #107C10; - box-shadow: 0 5px rgb(161, 161, 161); - transform: translateY(4px); + button:active { + box-shadow: 0 3px rgb(161, 161, 161); + transform: translateY(1px); } .clearfix::after { @@ -96,203 +105,337 @@ display: table; } - -

ROVER COMMAND CENTER

+ + + + + + +
-
-
-

Movement Control

-
- -
-
- - - -
+ +
+ - - + -
-
-

Sensor Data

-
    - -
  • -
    - -
    -
    - -
    -
    • - - -
  • -
    - -
    -
    - 28mm -
    -
    • - -
-
- -
+ + + + + +

Control Panel

Telemetry

Command Console

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Main
Set Heading to: 270°
Set Translation to: 180mm
Set Speed to: 50%
+ 		+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
X +
X,Y
H°
X mm
XV
X%
X
+ 		+ + + + + + + +

+ +
- + + + \ No newline at end of file diff --git a/Control/platformio.ini b/Control/platformio.ini index 0f97db2..b6309b8 100644 --- a/Control/platformio.ini +++ b/Control/platformio.ini @@ -13,7 +13,6 @@ platform = espressif32 board = esp32dev framework = arduino monitor_speed = 115200 -upload_port = COM[3] monitor_filters = send_on_enter esp32_exception_decoder diff --git a/Control/ref/drive.cpp b/Control/ref/drive.cpp index a04c7cd..6c9e467 100644 --- a/Control/ref/drive.cpp +++ b/Control/ref/drive.cpp @@ -19,12 +19,12 @@ void loop() deserializeJson(rdoc, Serial1); // Take JSON input from UART1 int requiredHeading = rdoc["rH"]; // if -1: command in progress, returning requested heading, dist/sp to be ignored int distance = rdoc["dist"]; // -1 for emergency stop - float speed = rdoc["sp"]; // -1 for emergency stop + float spd = rdoc["sp"]; // -1 for emergency stop int currentHeading = rdoc["cH"]; - bool resetDistanceTravelled = rdoc["rstD"]; + bool resetDistanceTravelled = rdoc["rstD"]; bool commandComplete = 0; - float powerUsage_mW = 0.0; + float powerUsage_mWh = 0.0; int distTravelled_mm = 0; int current_x = 0; int current_y = 0; @@ -33,7 +33,7 @@ void loop() DynamicJsonDocument tdoc(1024); // transmit doc, not sure how big this needs to be tdoc["comp"] = commandComplete; // If 0: command in progress, current heading requested - tdoc["mW"] = powerUsage_mW; + tdoc["mWh"] = powerUsage_mWh; tdoc["mm"] = distTravelled_mm; tdoc["pos"][0] = current_x; tdoc["pos"][1] = current_y; diff --git a/Control/src/command.jison b/Control/src/command.jison new file mode 100644 index 0000000..f36dfd9 --- /dev/null +++ b/Control/src/command.jison @@ -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' +<> 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(); + } + ; + diff --git a/Control/src/main.cpp b/Control/src/main.cpp index c86e7f7..1ad0ac4 100644 --- a/Control/src/main.cpp +++ b/Control/src/main.cpp @@ -1,6 +1,7 @@ #pragma region Includes #include #include +#include // Software Serial not currently needed #include #include #include @@ -22,12 +23,14 @@ #pragma endregion #pragma region Definitions eg pins -#define RX1pin 18 // Pin 6 on expansion board, UART1 -#define TX1pin 5 // Pin 7 on expansion board, UART1 -#define RX2pin 17 // Pin 8 on expansion board, UART2 -#define TX2pin 16 // Pin 9 on expansion board, UART2 +#define RX1pin 17 // Pin 6 on expansion board, UART1 +#define TX1pin 16 // Pin 7 on expansion board, UART1 +#define RX2pin 18 // Pin 8 on expansion board, UART2 +#define TX2pin 5 // Pin 9 on expansion board, UART2 #define RX3pin 14 // Pin 10 on expansion board, UART3 #define TX3pin 4 // Pin 11 on expansion board, UART3 +#define RX4pin 15 // Pin 12 on expansion board, UART4 +#define TX4pin 2 // Pin 13 on expansion board, UART4 #pragma endregion #pragma region Function Declarations @@ -41,6 +44,7 @@ void sendToEnergy(bool instruction); void recvFromEnergy(); void sendToVision(); void recvFromVision(); +void recvFromCompass(); void emergencyStop(); #pragma endregion @@ -48,7 +52,7 @@ void emergencyStop(); AsyncWebServer webserver(80); WebSocketsServer websocketserver(81); Ticker ticker; -SoftwareSerial Serial3; +SoftwareSerial Serial3, Serial4; std::queue InstrQueue; #pragma endregion @@ -75,10 +79,11 @@ void setup() esp_log_level_set("wifi", ESP_LOG_WARN); // enable WARN logs from WiFi stack esp_log_level_set("dhcpc", ESP_LOG_INFO); // enable INFO logs from DHCP client - Serial.begin(115200); // Set up hardware UART0 (Connected to USB port) - Serial1.begin(9600, SERIAL_8N1, RX1pin, TX1pin); // Set up hardware UART1 (Connected to Drive) - Serial2.begin(9600, SERIAL_8N1, RX2pin, TX2pin); // Set up hardware UART2 (Connected to Energy) - Serial3.begin(9600, SWSERIAL_8N1, RX3pin, TX3pin); // Set up software UART3 (Connected to Vision) + Serial.begin(115200); // Set up hardware UART0 (Connected to USB port) + Serial1.begin(9600, SERIAL_8N1, RX1pin, TX1pin); // Set up hardware UART1 (Connected to Drive) + Serial2.begin(9600, SERIAL_8N1, RX2pin, TX2pin); // Set up hardware UART2 (Connected to Energy) + Serial3.begin(9600, SWSERIAL_8N1, RX3pin, TX3pin); // Set up software UART3 (Connected to Vision) + Serial4.begin(9600, SWSERIAL_8N1, RX4pin, TX4pin); // Set up software UART4 (Connected to Compass) // Set global variable startup values Status = CS_IDLE; @@ -108,15 +113,17 @@ void setup() { delay(500); } - while (!MDNS.begin("rover")) // Set up mDNS cast at "rover.local/" + while (!MDNS.begin("rover2")) // Set up mDNS cast at "rover.local/" { Serial.println("Error setting up mDNS, retrying in 5s"); delay(5000); } - Serial.println("mDNS set up, access Control Panel at 'rover.local/'"); + Serial.println("mDNS set up, access Control Panel at 'rover2.local/'"); webserver.on("/", HTTP_GET, [](AsyncWebServerRequest *request) { request->send(SPIFFS, "/index.html", "text/html"); }); // Serve "index.html" at root page + webserver.on("/command.js", HTTP_GET, [](AsyncWebServerRequest *request) + { request->send(SPIFFS, "/command.js", "text/js"); }); // Serve "command.js" for root page to accessj webserver.on("/favicon.ico", HTTP_GET, [](AsyncWebServerRequest *request) { request->send(SPIFFS, "/favicon.ico", "image/png"); }); // Serve tab icon webserver.onNotFound(notFound); // Set up basic 404NotFound page @@ -132,7 +139,8 @@ void loop() websocketserver.loop(); // Handle incoming client connections recvFromDrive(); // Update stats from Drive recvFromEnergy(); // Update stats from Energy - recvFromVision(); // Update stats from Vision + // recvFromVision(); // Update stats from Vision + recvFromCompass(); // Update stats from Compass switch (Status) { case CS_ERROR: @@ -195,6 +203,7 @@ void loop() break; } lastExecutedCommand = instr->id; // Update tracker of last processed command + InstrQueue.pop(); } } break; @@ -242,7 +251,7 @@ void loop() } break; } - delay(500); + // delay(500); } void notFound(AsyncWebServerRequest *request) @@ -447,13 +456,20 @@ void recvFromVision() // Update bounding box and obstacle detection data from Vi } } +void recvFromCompass() +{ + if (Serial4.available()) + { + DynamicJsonDocument rdoc(1024); + deserializeJson(rdoc, Serial4); + heading = rdoc["cH"]; + } +} + void emergencyStop() { DynamicJsonDocument tdoc(1024); - tdoc["rH"] = heading; - tdoc["dist"] = -1; - tdoc["sp"] = -1; - tdoc["cH"] = heading; + tdoc["stp"] = 1; serializeJson(tdoc, Serial1); // Send stop signals to Drive sendToEnergy(0); // Send stop signal to Energy while (InstrQueue.size()) diff --git a/Drive/.gitignore b/Drive/.gitignore new file mode 100644 index 0000000..89cc49c --- /dev/null +++ b/Drive/.gitignore @@ -0,0 +1,5 @@ +.pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch diff --git a/Drive/.vscode/extensions.json b/Drive/.vscode/extensions.json new file mode 100644 index 0000000..e80666b --- /dev/null +++ b/Drive/.vscode/extensions.json @@ -0,0 +1,7 @@ +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "platformio.platformio-ide" + ] +} diff --git a/Drive/include/README b/Drive/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/Drive/include/README @@ -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 diff --git a/Drive/lib/README b/Drive/lib/README new file mode 100644 index 0000000..6debab1 --- /dev/null +++ b/Drive/lib/README @@ -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 +#include + +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 diff --git a/Drive/platformio.ini b/Drive/platformio.ini new file mode 100644 index 0000000..e30e663 --- /dev/null +++ b/Drive/platformio.ini @@ -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 diff --git a/Drive/src/main.cpp b/Drive/src/main.cpp new file mode 100644 index 0000000..da06bdf --- /dev/null +++ b/Drive/src/main.cpp @@ -0,0 +1,872 @@ +#include +#include +// #include +#include +#include +#include "SPI.h" +#include + +// #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; +} \ No newline at end of file diff --git a/Drive/test/README b/Drive/test/README new file mode 100644 index 0000000..b94d089 --- /dev/null +++ b/Drive/test/README @@ -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 diff --git a/IMU/.gitignore b/IMU/.gitignore new file mode 100644 index 0000000..89cc49c --- /dev/null +++ b/IMU/.gitignore @@ -0,0 +1,5 @@ +.pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch diff --git a/IMU/.vscode/extensions.json b/IMU/.vscode/extensions.json new file mode 100644 index 0000000..e80666b --- /dev/null +++ b/IMU/.vscode/extensions.json @@ -0,0 +1,7 @@ +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "platformio.platformio-ide" + ] +} diff --git a/IMU/include/README b/IMU/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/IMU/include/README @@ -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 diff --git a/IMU/lib/README b/IMU/lib/README new file mode 100644 index 0000000..6debab1 --- /dev/null +++ b/IMU/lib/README @@ -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 +#include + +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 diff --git a/IMU/platformio.ini b/IMU/platformio.ini new file mode 100644 index 0000000..f89137d --- /dev/null +++ b/IMU/platformio.ini @@ -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 diff --git a/IMU/src/main.cpp b/IMU/src/main.cpp new file mode 100644 index 0000000..c04011f --- /dev/null +++ b/IMU/src/main.cpp @@ -0,0 +1,25 @@ +#include +#include +#include + +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); +} \ No newline at end of file diff --git a/IMU/test/README b/IMU/test/README new file mode 100644 index 0000000..b94d089 --- /dev/null +++ b/IMU/test/README @@ -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