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

main.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 "commandhandler.h"
dralisz82 0:e5ee05ce9e0c 4 #include "robotkocsi/drive.h"
dralisz82 0:e5ee05ce9e0c 5 #include "robotkocsi/lights.h"
dralisz82 0:e5ee05ce9e0c 6 #include "robotkocsi/sensors.h"
dralisz82 0:e5ee05ce9e0c 7 #include "robotkocsi/demo.h"
dralisz82 0:e5ee05ce9e0c 8
dralisz82 0:e5ee05ce9e0c 9
dralisz82 0:e5ee05ce9e0c 10 DigitalOut heartbeatLED(LED_GREEN);
dralisz82 0:e5ee05ce9e0c 11 DigitalOut cmdExecLED(LED_RED);
dralisz82 0:e5ee05ce9e0c 12
dralisz82 0:e5ee05ce9e0c 13 Lights* lights;
dralisz82 0:e5ee05ce9e0c 14 Drive* drive;
dralisz82 0:e5ee05ce9e0c 15 Sensors* sensors;
dralisz82 0:e5ee05ce9e0c 16 Demo* demo;
dralisz82 0:e5ee05ce9e0c 17
dralisz82 0:e5ee05ce9e0c 18 Serial BT(PTC15, PTC14); // Bluetooth module header
dralisz82 0:e5ee05ce9e0c 19
dralisz82 0:e5ee05ce9e0c 20 Thread *hbThread; // heartbeat thread
dralisz82 0:e5ee05ce9e0c 21 //Thread *nw_thread; // responsible for ethernet network
dralisz82 0:e5ee05ce9e0c 22 extern Thread* cmdHandlerThread;
dralisz82 0:e5ee05ce9e0c 23
dralisz82 0:e5ee05ce9e0c 24 void execCommand(char *cmd, int argc, simplestr *args) {
dralisz82 0:e5ee05ce9e0c 25 if(!strcmp(cmd, "forward")) {
dralisz82 0:e5ee05ce9e0c 26 drive->forward();// lights->reversingLightOff();
dralisz82 0:e5ee05ce9e0c 27 }
dralisz82 0:e5ee05ce9e0c 28 else if(!strcmp(cmd, "backward")) {
dralisz82 0:e5ee05ce9e0c 29 drive->backward();// lights->reversingLightOn();
dralisz82 0:e5ee05ce9e0c 30 }
dralisz82 0:e5ee05ce9e0c 31 else if(!strcmp(cmd, "halt")) {
dralisz82 0:e5ee05ce9e0c 32 drive->stop();// lights->reversingLightOff();
dralisz82 0:e5ee05ce9e0c 33 }
dralisz82 0:e5ee05ce9e0c 34 else if(!strcmp(cmd, "left")) {
dralisz82 0:e5ee05ce9e0c 35 drive->steerLeft();// lights->indexLeft();
dralisz82 0:e5ee05ce9e0c 36 }
dralisz82 0:e5ee05ce9e0c 37 else if(!strcmp(cmd, "right")) {
dralisz82 0:e5ee05ce9e0c 38 drive->steerRight();// lights->indexRight();
dralisz82 0:e5ee05ce9e0c 39 }
dralisz82 0:e5ee05ce9e0c 40 else if(!strcmp(cmd, "straight")) {
dralisz82 0:e5ee05ce9e0c 41 drive->steerStraight();// lights->indexOff();
dralisz82 0:e5ee05ce9e0c 42 }
dralisz82 0:e5ee05ce9e0c 43 else if(!strcmp(cmd, "index_left")) {
dralisz82 0:e5ee05ce9e0c 44 lights->indexLeft();
dralisz82 0:e5ee05ce9e0c 45 }
dralisz82 0:e5ee05ce9e0c 46 else if(!strcmp(cmd, "index_right")) {
dralisz82 0:e5ee05ce9e0c 47 lights->indexRight();
dralisz82 0:e5ee05ce9e0c 48 }
dralisz82 0:e5ee05ce9e0c 49 else if(!strcmp(cmd, "index_off")) {
dralisz82 0:e5ee05ce9e0c 50 lights->indexOff();
dralisz82 0:e5ee05ce9e0c 51 }
dralisz82 0:e5ee05ce9e0c 52 else if(!strcmp(cmd, "lights")) {
dralisz82 0:e5ee05ce9e0c 53 lights->headLightToggle();
dralisz82 0:e5ee05ce9e0c 54 }
dralisz82 0:e5ee05ce9e0c 55 else if(!strcmp(cmd, "high_beam")) {
dralisz82 0:e5ee05ce9e0c 56 lights->highBeamToggle();
dralisz82 0:e5ee05ce9e0c 57 }
dralisz82 0:e5ee05ce9e0c 58 else if(!strcmp(cmd, "demo_play")) {
dralisz82 0:e5ee05ce9e0c 59 demo->playPause();
dralisz82 0:e5ee05ce9e0c 60 }
dralisz82 0:e5ee05ce9e0c 61 else if(!strcmp(cmd, "demo_reset")) {
dralisz82 0:e5ee05ce9e0c 62 demo->reset();
dralisz82 0:e5ee05ce9e0c 63 }
dralisz82 0:e5ee05ce9e0c 64 else if(!strcmp(cmd, "readSensor")) {
dralisz82 0:e5ee05ce9e0c 65 Sensor *s = sensors->getSensor(args[0]);
dralisz82 0:e5ee05ce9e0c 66 if(s != NULL) {
dralisz82 0:e5ee05ce9e0c 67 BT.printf("%s: %f %s\n", s->getName(), s->readValue(), s->getMetric());
dralisz82 0:e5ee05ce9e0c 68 }
dralisz82 0:e5ee05ce9e0c 69 }
dralisz82 0:e5ee05ce9e0c 70 else {
dralisz82 0:e5ee05ce9e0c 71 lights->hazardLightsOn(); // ignore invalid command, put emergency indicator on
dralisz82 0:e5ee05ce9e0c 72 }
dralisz82 0:e5ee05ce9e0c 73 printf("%s %d; %s, %s, %s, %s\n", cmd, argc, args[0], args[1], args[2], args[3]);
dralisz82 0:e5ee05ce9e0c 74 }
dralisz82 0:e5ee05ce9e0c 75
dralisz82 0:e5ee05ce9e0c 76 /**
dralisz82 0:e5ee05ce9e0c 77 * Heartbeat thread main loop
dralisz82 0:e5ee05ce9e0c 78 */
dralisz82 0:e5ee05ce9e0c 79 void hbThreadMain(void const *argument) {
dralisz82 0:e5ee05ce9e0c 80 while (true) {
dralisz82 0:e5ee05ce9e0c 81 heartbeatLED = 0;
dralisz82 0:e5ee05ce9e0c 82 Thread::wait(100);
dralisz82 0:e5ee05ce9e0c 83 heartbeatLED = 1;
dralisz82 0:e5ee05ce9e0c 84 Thread::wait(1900);
dralisz82 0:e5ee05ce9e0c 85 }
dralisz82 0:e5ee05ce9e0c 86 }
dralisz82 0:e5ee05ce9e0c 87
dralisz82 0:e5ee05ce9e0c 88
dralisz82 0:e5ee05ce9e0c 89 /**
dralisz82 0:e5ee05ce9e0c 90 * Network thread main loop
dralisz82 0:e5ee05ce9e0c 91 */
dralisz82 0:e5ee05ce9e0c 92 /*void nw_thread_main(void const *argument) {
dralisz82 0:e5ee05ce9e0c 93 EthernetInterface eth;
dralisz82 0:e5ee05ce9e0c 94 eth.init(); //Use DHCP
dralisz82 0:e5ee05ce9e0c 95 eth.connect();
dralisz82 0:e5ee05ce9e0c 96 printf("IP Address is %s\n", eth.getIPAddress());
dralisz82 0:e5ee05ce9e0c 97
dralisz82 0:e5ee05ce9e0c 98 TCPSocketConnection sock;
dralisz82 0:e5ee05ce9e0c 99 sock.connect("mbed.org", 80);
dralisz82 0:e5ee05ce9e0c 100
dralisz82 0:e5ee05ce9e0c 101 char http_cmd[] = "GET /media/uploads/mbed_official/hello.txt HTTP/1.0\n\n";
dralisz82 0:e5ee05ce9e0c 102 sock.send_all(http_cmd, sizeof(http_cmd)-1);
dralisz82 0:e5ee05ce9e0c 103
dralisz82 0:e5ee05ce9e0c 104 char buffer[300];
dralisz82 0:e5ee05ce9e0c 105 int ret;
dralisz82 0:e5ee05ce9e0c 106 while (true) {
dralisz82 0:e5ee05ce9e0c 107 ret = sock.receive(buffer, sizeof(buffer)-1);
dralisz82 0:e5ee05ce9e0c 108 if (ret <= 0)
dralisz82 0:e5ee05ce9e0c 109 break;
dralisz82 0:e5ee05ce9e0c 110 buffer[ret] = '\0';
dralisz82 0:e5ee05ce9e0c 111 printf("Received %d chars from server:\n%s\n", ret, buffer);
dralisz82 0:e5ee05ce9e0c 112 }
dralisz82 0:e5ee05ce9e0c 113
dralisz82 0:e5ee05ce9e0c 114 sock.close();
dralisz82 0:e5ee05ce9e0c 115
dralisz82 0:e5ee05ce9e0c 116 eth.disconnect();
dralisz82 0:e5ee05ce9e0c 117 }*/
dralisz82 0:e5ee05ce9e0c 118
dralisz82 0:e5ee05ce9e0c 119 int main() {
dralisz82 0:e5ee05ce9e0c 120 printf("OS Started\n");
dralisz82 0:e5ee05ce9e0c 121
dralisz82 0:e5ee05ce9e0c 122 // Init peripherals
dralisz82 0:e5ee05ce9e0c 123 heartbeatLED = 1;
dralisz82 0:e5ee05ce9e0c 124 cmdExecLED = 1;
dralisz82 0:e5ee05ce9e0c 125
dralisz82 0:e5ee05ce9e0c 126 BT.baud(9600); // HC-05 module works at this rate by default
dralisz82 0:e5ee05ce9e0c 127 BT.attach(&gotChar); // register interrupt handler
dralisz82 0:e5ee05ce9e0c 128
dralisz82 0:e5ee05ce9e0c 129 // index bal: PTA1 / D3
dralisz82 0:e5ee05ce9e0c 130 // index jobb: PTB9 / D2
dralisz82 0:e5ee05ce9e0c 131 // nappali fény: PTC3 / D7
dralisz82 0:e5ee05ce9e0c 132 // reflektor: PTC2 / D6
dralisz82 0:e5ee05ce9e0c 133 // tolatólámpa: PTB23 / D4
dralisz82 0:e5ee05ce9e0c 134 // féklámpa: PTA2 / D5
dralisz82 0:e5ee05ce9e0c 135 lights = new Lights(PTA1, PTB9, PTC3, PTC2, PTB23, PTA2);
dralisz82 0:e5ee05ce9e0c 136
dralisz82 0:e5ee05ce9e0c 137 // előre: PTD0 / D10
dralisz82 0:e5ee05ce9e0c 138 // hátra: PTD1 / D13
dralisz82 0:e5ee05ce9e0c 139 // balra: PTD2 / D11
dralisz82 0:e5ee05ce9e0c 140 // jobbra: PTD3 / D12
dralisz82 0:e5ee05ce9e0c 141 // passing lights, thus turning on automatic index and reversing lights
dralisz82 0:e5ee05ce9e0c 142 drive = new Drive(PTD0, PTD1, PTD2, PTD3, lights);
dralisz82 0:e5ee05ce9e0c 143
dralisz82 0:e5ee05ce9e0c 144 // simple demo of autonomous operation (currently sensorless, timing based)
dralisz82 0:e5ee05ce9e0c 145 demo = new Demo(drive, lights);
dralisz82 0:e5ee05ce9e0c 146
dralisz82 0:e5ee05ce9e0c 147 // sensor objects can be obtained from this object
dralisz82 0:e5ee05ce9e0c 148 sensors = new Sensors();
dralisz82 0:e5ee05ce9e0c 149
dralisz82 0:e5ee05ce9e0c 150 printf("Starting threads\n");
dralisz82 0:e5ee05ce9e0c 151
dralisz82 0:e5ee05ce9e0c 152 hbThread = new Thread(hbThreadMain);
dralisz82 0:e5ee05ce9e0c 153 // nw_thread = new Thread(nw_thread_main);
dralisz82 0:e5ee05ce9e0c 154 cmdHandlerThread = new Thread(cmdHandlerMain);
dralisz82 0:e5ee05ce9e0c 155
dralisz82 0:e5ee05ce9e0c 156 // main control loop
dralisz82 0:e5ee05ce9e0c 157 while(1) {
dralisz82 0:e5ee05ce9e0c 158 // Currently not doing anything here
dralisz82 0:e5ee05ce9e0c 159 Thread::wait(2000);
dralisz82 0:e5ee05ce9e0c 160 }
dralisz82 0:e5ee05ce9e0c 161 printf("End of main (never should be executed)\n");
dralisz82 0:e5ee05ce9e0c 162 }