Szilárd Greszler / Mbed OS robotkocsi_OS

Controller firmware for a mobile robot, having a K64F MCU on board. Please see README.md for details.

Dependencies:   robotkocsi

commandhandler.cpp@0:e5ee05ce9e0c, 2018-05-30 (annotated)

Committer:
dralisz82
Date:
Wed May 30 15:10:48 2018 +0000
Revision:
0:e5ee05ce9e0c
Controller firmware for a mobile robot, having a K64F MCU on board.; ; Please read README.md for details.;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dralisz82 0:e5ee05ce9e0c 1 #include "mbed.h"
dralisz82 0:e5ee05ce9e0c 2 #include "rtos.h"
dralisz82 0:e5ee05ce9e0c 3 #include <ctype.h>
dralisz82 0:e5ee05ce9e0c 4 #include "commandhandler.h"
dralisz82 0:e5ee05ce9e0c 5
dralisz82 0:e5ee05ce9e0c 6 char bt_receive_buffer[BT_RB_SIZE];
dralisz82 0:e5ee05ce9e0c 7 unsigned int bt_rb_pos = 0;
dralisz82 0:e5ee05ce9e0c 8
dralisz82 0:e5ee05ce9e0c 9 Mail<simplestr, 16> cmdMailBox;
dralisz82 0:e5ee05ce9e0c 10 Thread* cmdHandlerThread;
dralisz82 0:e5ee05ce9e0c 11
dralisz82 0:e5ee05ce9e0c 12 extern Serial BT;
dralisz82 0:e5ee05ce9e0c 13 extern DigitalOut cmdExecLED;
dralisz82 0:e5ee05ce9e0c 14 /**
dralisz82 0:e5ee05ce9e0c 15 * UART Interrupt handler
dralisz82 0:e5ee05ce9e0c 16 */
dralisz82 0:e5ee05ce9e0c 17 void gotChar() {
dralisz82 0:e5ee05ce9e0c 18 simplestr *mail;
dralisz82 0:e5ee05ce9e0c 19 char c = BT.getc(); // read incoming character
dralisz82 0:e5ee05ce9e0c 20
dralisz82 0:e5ee05ce9e0c 21 if(c != '\n')
dralisz82 0:e5ee05ce9e0c 22 bt_receive_buffer[bt_rb_pos++] = c; // receive characters till EOL
dralisz82 0:e5ee05ce9e0c 23 else
dralisz82 0:e5ee05ce9e0c 24 bt_receive_buffer[bt_rb_pos++] = '\0'; // replace EOL with null terminator
dralisz82 0:e5ee05ce9e0c 25
dralisz82 0:e5ee05ce9e0c 26 if(c == '\n' || bt_rb_pos == BT_RB_SIZE) { // if EOL or buffer is full...
dralisz82 0:e5ee05ce9e0c 27 if(bt_rb_pos == BT_RB_SIZE)
dralisz82 0:e5ee05ce9e0c 28 bt_receive_buffer[BT_RB_SIZE - 1] = '\0'; // Better would be some error handling here
dralisz82 0:e5ee05ce9e0c 29 cmdExecLED = 0;
dralisz82 0:e5ee05ce9e0c 30 mail = cmdMailBox.alloc(2); // 2 ms timeout
dralisz82 0:e5ee05ce9e0c 31 if(mail != NULL) {
dralisz82 0:e5ee05ce9e0c 32 strcpy(*mail, bt_receive_buffer);
dralisz82 0:e5ee05ce9e0c 33 cmdMailBox.put(mail);
dralisz82 0:e5ee05ce9e0c 34 // handleCommand();
dralisz82 0:e5ee05ce9e0c 35 }
dralisz82 0:e5ee05ce9e0c 36 bt_rb_pos = 0;
dralisz82 0:e5ee05ce9e0c 37 }
dralisz82 0:e5ee05ce9e0c 38
dralisz82 0:e5ee05ce9e0c 39 // interpret received character as a command to switch color
dralisz82 0:e5ee05ce9e0c 40 // if(c == 'r' || c == 'g' || c == 'b')
dralisz82 0:e5ee05ce9e0c 41 // color = c;
dralisz82 0:e5ee05ce9e0c 42 }
dralisz82 0:e5ee05ce9e0c 43
dralisz82 0:e5ee05ce9e0c 44 // Note: This function returns a pointer to a substring of the original string.
dralisz82 0:e5ee05ce9e0c 45 // If the given string was allocated dynamically, the caller must not overwrite
dralisz82 0:e5ee05ce9e0c 46 // that pointer with the returned value, since the original pointer must be
dralisz82 0:e5ee05ce9e0c 47 // deallocated using the same allocator with which it was allocated. The return
dralisz82 0:e5ee05ce9e0c 48 // value must NOT be deallocated using free() etc.
dralisz82 0:e5ee05ce9e0c 49 char *trimwhitespace(char *str)
dralisz82 0:e5ee05ce9e0c 50 {
dralisz82 0:e5ee05ce9e0c 51 char *end;
dralisz82 0:e5ee05ce9e0c 52
dralisz82 0:e5ee05ce9e0c 53 // Trim leading space
dralisz82 0:e5ee05ce9e0c 54 while(isspace((unsigned char)*str)) str++;
dralisz82 0:e5ee05ce9e0c 55
dralisz82 0:e5ee05ce9e0c 56 if(*str == 0) // All spaces?
dralisz82 0:e5ee05ce9e0c 57 return str;
dralisz82 0:e5ee05ce9e0c 58
dralisz82 0:e5ee05ce9e0c 59 // Trim trailing space
dralisz82 0:e5ee05ce9e0c 60 end = str + strlen(str) - 1;
dralisz82 0:e5ee05ce9e0c 61 while(end > str && isspace((unsigned char)*end)) end--;
dralisz82 0:e5ee05ce9e0c 62
dralisz82 0:e5ee05ce9e0c 63 // Write new null terminator
dralisz82 0:e5ee05ce9e0c 64 *(end+1) = 0;
dralisz82 0:e5ee05ce9e0c 65
dralisz82 0:e5ee05ce9e0c 66 return str;
dralisz82 0:e5ee05ce9e0c 67 }
dralisz82 0:e5ee05ce9e0c 68
dralisz82 0:e5ee05ce9e0c 69 void handleCommand() {
dralisz82 0:e5ee05ce9e0c 70
dralisz82 0:e5ee05ce9e0c 71 char *trimmedCommand;
dralisz82 0:e5ee05ce9e0c 72 const char separator[2] = " ";
dralisz82 0:e5ee05ce9e0c 73 char *token;
dralisz82 0:e5ee05ce9e0c 74
dralisz82 0:e5ee05ce9e0c 75 char cmd[BT_RB_SIZE];
dralisz82 0:e5ee05ce9e0c 76 char args[4][BT_RB_SIZE] = {0};
dralisz82 0:e5ee05ce9e0c 77 int argIdx = 0;
dralisz82 0:e5ee05ce9e0c 78
dralisz82 0:e5ee05ce9e0c 79 osEvent evt = cmdMailBox.get();
dralisz82 0:e5ee05ce9e0c 80 if (evt.status != osEventMail)
dralisz82 0:e5ee05ce9e0c 81 return;
dralisz82 0:e5ee05ce9e0c 82
dralisz82 0:e5ee05ce9e0c 83 simplestr *mail = (simplestr*)evt.value.p;
dralisz82 0:e5ee05ce9e0c 84 printf("cmd: %s\n", mail);
dralisz82 0:e5ee05ce9e0c 85 trimmedCommand = trimwhitespace(*mail);
dralisz82 0:e5ee05ce9e0c 86
dralisz82 0:e5ee05ce9e0c 87 printf("tcmd: >%s<\n", trimmedCommand);
dralisz82 0:e5ee05ce9e0c 88
dralisz82 0:e5ee05ce9e0c 89
dralisz82 0:e5ee05ce9e0c 90 /* get the first token */
dralisz82 0:e5ee05ce9e0c 91 token = strtok(trimmedCommand, separator); // TODO: replace with strtok_r()
dralisz82 0:e5ee05ce9e0c 92
dralisz82 0:e5ee05ce9e0c 93 if(token == NULL)
dralisz82 0:e5ee05ce9e0c 94 return;
dralisz82 0:e5ee05ce9e0c 95
dralisz82 0:e5ee05ce9e0c 96 strcpy(cmd, token);
dralisz82 0:e5ee05ce9e0c 97
dralisz82 0:e5ee05ce9e0c 98 /* walk through arguments */
dralisz82 0:e5ee05ce9e0c 99 while( (token = strtok(NULL, separator)) != NULL ) {
dralisz82 0:e5ee05ce9e0c 100 strcpy(args[argIdx++], token);
dralisz82 0:e5ee05ce9e0c 101 }
dralisz82 0:e5ee05ce9e0c 102
dralisz82 0:e5ee05ce9e0c 103 cmdMailBox.free(mail);
dralisz82 0:e5ee05ce9e0c 104 execCommand(cmd, argIdx, args);
dralisz82 0:e5ee05ce9e0c 105 cmdExecLED = 1;
dralisz82 0:e5ee05ce9e0c 106 }
dralisz82 0:e5ee05ce9e0c 107
dralisz82 0:e5ee05ce9e0c 108 void cmdHandlerMain(void const *argument) {
dralisz82 0:e5ee05ce9e0c 109 printf("Command handler started\n");
dralisz82 0:e5ee05ce9e0c 110 while(1) {
dralisz82 0:e5ee05ce9e0c 111 handleCommand();
dralisz82 0:e5ee05ce9e0c 112 // Thread::wait(2000); // For testing purposes only
dralisz82 0:e5ee05ce9e0c 113 }
dralisz82 0:e5ee05ce9e0c 114 }