Nicolas Borla / Mbed OS ROME2_Robot_Firmware

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Main.cpp@0:4beb2ea291ec, 2020-03-16 (annotated)

Committer:
boro
Date:
Mon Mar 16 13:12:31 2020 +0000
Revision:
0:4beb2ea291ec
Child:
1:94c549ec20f9
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
boro 0:4beb2ea291ec 1 /*
boro 0:4beb2ea291ec 2 * Main.cpp
boro 0:4beb2ea291ec 3 * Copyright (c) 2017, ZHAW
boro 0:4beb2ea291ec 4 * All rights reserved.
boro 0:4beb2ea291ec 5 *
boro 0:4beb2ea291ec 6 * Created on: 07.07.2017
boro 0:4beb2ea291ec 7 * Author: Marcel Honegger
boro 0:4beb2ea291ec 8 */
boro 0:4beb2ea291ec 9
boro 0:4beb2ea291ec 10 #include <mbed.h>
boro 0:4beb2ea291ec 11 #include <EthernetInterface.h>
boro 0:4beb2ea291ec 12 //#include "SDFileSystem/SDFileSystem.h"
boro 0:4beb2ea291ec 13 #include "HTTPServer.h"
boro 0:4beb2ea291ec 14 #include "IndexScript.h"
boro 0:4beb2ea291ec 15 #include "ControllerScript.h"
boro 0:4beb2ea291ec 16 #include "EncoderCounter.h"
boro 0:4beb2ea291ec 17 #include "IMU.h"
boro 0:4beb2ea291ec 18 #include "IRSensor.h"
boro 0:4beb2ea291ec 19 #include "SDBlockDevice.h"
boro 0:4beb2ea291ec 20 #include "FATFileSystem.h"
boro 0:4beb2ea291ec 21 #include "IRSensor.h"
boro 0:4beb2ea291ec 22 #include "Controller.h"
boro 0:4beb2ea291ec 23 #include "LIDAR.h"
boro 0:4beb2ea291ec 24
boro 0:4beb2ea291ec 25 class Main {
boro 0:4beb2ea291ec 26
boro 0:4beb2ea291ec 27 public:
boro 0:4beb2ea291ec 28
boro 0:4beb2ea291ec 29 Main();
boro 0:4beb2ea291ec 30 void run();
boro 0:4beb2ea291ec 31
boro 0:4beb2ea291ec 32 private:
boro 0:4beb2ea291ec 33
boro 0:4beb2ea291ec 34 Thread thread;
boro 0:4beb2ea291ec 35 };
boro 0:4beb2ea291ec 36
boro 0:4beb2ea291ec 37 Main::Main() : thread(osPriorityNormal, 64*1024) {
boro 0:4beb2ea291ec 38
boro 0:4beb2ea291ec 39 thread.start(callback(this, &Main::run));
boro 0:4beb2ea291ec 40 }
boro 0:4beb2ea291ec 41
boro 0:4beb2ea291ec 42 void Main::run() {
boro 0:4beb2ea291ec 43
boro 0:4beb2ea291ec 44 // create user_button object
boro 0:4beb2ea291ec 45 DigitalIn user_button(USER_BUTTON);
boro 0:4beb2ea291ec 46 DigitalOut led(LED1);
boro 0:4beb2ea291ec 47 DigitalOut myLed2(LED2);
boro 0:4beb2ea291ec 48
boro 0:4beb2ea291ec 49 DigitalOut enableRouter(PB_15);
boro 0:4beb2ea291ec 50 enableRouter = 1;
boro 0:4beb2ea291ec 51
boro 0:4beb2ea291ec 52 printf("Starting Router\r\n");
boro 0:4beb2ea291ec 53 Thread::wait(1000);
boro 0:4beb2ea291ec 54 printf("Starting webserver\r\n");
boro 0:4beb2ea291ec 55
boro 0:4beb2ea291ec 56 // create webserver
boro 0:4beb2ea291ec 57 EthernetInterface* ethernet = new EthernetInterface();
boro 0:4beb2ea291ec 58 ethernet->set_network("192.168.0.100", "255.255.255.0", "192.168.1.1");
boro 0:4beb2ea291ec 59 ethernet->connect();
boro 0:4beb2ea291ec 60 printf("The target IP address is '%s'\r\n", ethernet->get_ip_address());
boro 0:4beb2ea291ec 61
boro 0:4beb2ea291ec 62 HTTPServer httpServer(*ethernet);
boro 0:4beb2ea291ec 63
boro 0:4beb2ea291ec 64
boro 0:4beb2ea291ec 65
boro 0:4beb2ea291ec 66
boro 0:4beb2ea291ec 67 // initialise PWM objects
boro 0:4beb2ea291ec 68 PwmOut pwmRight(PF_8);
boro 0:4beb2ea291ec 69 PwmOut pwmLeft(PF_7);
boro 0:4beb2ea291ec 70 PwmOut pwmReserve(PF_9);
boro 0:4beb2ea291ec 71
boro 0:4beb2ea291ec 72 // crete Encoder read objects
boro 0:4beb2ea291ec 73 EncoderCounter counterRight(PB_4, PC_7); // counter(pin A, pin B)
boro 0:4beb2ea291ec 74 EncoderCounter counterLeft(PA_15, PB_3);
boro 0:4beb2ea291ec 75 EncoderCounter counterRes(PD_12, PD_13);
boro 0:4beb2ea291ec 76
boro 0:4beb2ea291ec 77 PwmOut servo1(PE_10);
boro 0:4beb2ea291ec 78 PwmOut servo2(PE_12);
boro 0:4beb2ea291ec 79
boro 0:4beb2ea291ec 80 servo1.period(0.02f);// 0.02ms 50Hz
boro 0:4beb2ea291ec 81 servo2.period(0.02f);// 0.02ms 50Hz
boro 0:4beb2ea291ec 82 servo1.write(0.01f);
boro 0:4beb2ea291ec 83 servo2.write(0.01f);
boro 0:4beb2ea291ec 84
boro 0:4beb2ea291ec 85 Controller controller(pwmReserve, pwmRight, counterRes, counterRight);
boro 0:4beb2ea291ec 86
boro 0:4beb2ea291ec 87 // create LIDAR object
boro 0:4beb2ea291ec 88 PwmOut pwm(PE_9);
boro 0:4beb2ea291ec 89 pwm.period(0.00005f);
boro 0:4beb2ea291ec 90 pwm.write(0.5f); // 50% duty-cycle
boro 0:4beb2ea291ec 91
boro 0:4beb2ea291ec 92 RawSerial com(PG_14, PG_9);
boro 0:4beb2ea291ec 93
boro 0:4beb2ea291ec 94 LIDAR lidar(com);
boro 0:4beb2ea291ec 95
boro 0:4beb2ea291ec 96 httpServer.add("index", new IndexScript());
boro 0:4beb2ea291ec 97 httpServer.add("controller", new ControllerScript(user_button,myLed2,controller,servo1,servo2,lidar));
boro 0:4beb2ea291ec 98
boro 0:4beb2ea291ec 99 // create IMU comunication objects
boro 0:4beb2ea291ec 100 SPI spi(PC_12, PC_11, PC_10); // mosi, miso, sclk
boro 0:4beb2ea291ec 101 DigitalOut csAG(PC_8);
boro 0:4beb2ea291ec 102 DigitalOut csM(PC_9);
boro 0:4beb2ea291ec 103
boro 0:4beb2ea291ec 104 //create IMU object
boro 0:4beb2ea291ec 105 IMU imu(spi, csAG, csM);
boro 0:4beb2ea291ec 106 printf("IMU init done\r\n");
boro 0:4beb2ea291ec 107 // create IRSensor object
boro 0:4beb2ea291ec 108 AnalogIn distance(PA_0);
boro 0:4beb2ea291ec 109
boro 0:4beb2ea291ec 110 // create multiplexer objects
boro 0:4beb2ea291ec 111 DigitalOut bit0(PF_0);
boro 0:4beb2ea291ec 112 DigitalOut bit1(PF_1);
boro 0:4beb2ea291ec 113 DigitalOut bit2(PF_2);
boro 0:4beb2ea291ec 114
boro 0:4beb2ea291ec 115 // create sd card object
boro 0:4beb2ea291ec 116 SDBlockDevice sd(PE_6, PE_5, PE_2, PE_4); // mosi miso clk cs
boro 0:4beb2ea291ec 117 FATFileSystem fs("sd");
boro 0:4beb2ea291ec 118 printf("SD FileSystem init done\r\n");
boro 0:4beb2ea291ec 119
boro 0:4beb2ea291ec 120 // others
boro 0:4beb2ea291ec 121 DigitalOut enable_motors(PG_0);
boro 0:4beb2ea291ec 122 DigitalIn driver_warning(PD_0);
boro 0:4beb2ea291ec 123 DigitalIn driver_fault(PD_1);
boro 0:4beb2ea291ec 124
boro 0:4beb2ea291ec 125 DigitalOut enable_IRsens(PG_1);
boro 0:4beb2ea291ec 126
boro 0:4beb2ea291ec 127 CAN can(PB_12, PB_6);
boro 0:4beb2ea291ec 128
boro 0:4beb2ea291ec 129 AnalogIn ain1(PB_1);
boro 0:4beb2ea291ec 130 AnalogIn ain2(PC_2);
boro 0:4beb2ea291ec 131 AnalogIn ain3(PF_4);
boro 0:4beb2ea291ec 132
boro 0:4beb2ea291ec 133 DigitalIn din1(PE_14);
boro 0:4beb2ea291ec 134 DigitalIn din2(PE_15);
boro 0:4beb2ea291ec 135 DigitalIn din3(PE_0);
boro 0:4beb2ea291ec 136
boro 0:4beb2ea291ec 137 I2C i2c(PB_11,PB_10);
boro 0:4beb2ea291ec 138
boro 0:4beb2ea291ec 139
boro 0:4beb2ea291ec 140
boro 0:4beb2ea291ec 141 Serial uartGroove(PE_8, PE_7); // tx, rx
boro 0:4beb2ea291ec 142
boro 0:4beb2ea291ec 143 DigitalOut led0(PD_4);
boro 0:4beb2ea291ec 144 DigitalOut led1(PD_3);
boro 0:4beb2ea291ec 145 DigitalOut led2(PD_6);
boro 0:4beb2ea291ec 146 DigitalOut led3(PD_2);
boro 0:4beb2ea291ec 147 DigitalOut led4(PD_7);
boro 0:4beb2ea291ec 148 DigitalOut led5(PD_5);
boro 0:4beb2ea291ec 149
boro 0:4beb2ea291ec 150 led0 = 0;
boro 0:4beb2ea291ec 151 led1 = 0;
boro 0:4beb2ea291ec 152 led2 = 0;
boro 0:4beb2ea291ec 153 led3 = 0;
boro 0:4beb2ea291ec 154 led4 = 0;
boro 0:4beb2ea291ec 155 led5 = 0;
boro 0:4beb2ea291ec 156
boro 0:4beb2ea291ec 157 enable_IRsens = 1;
boro 0:4beb2ea291ec 158
boro 0:4beb2ea291ec 159 IRSensor irSensor0(distance, bit0, bit1, bit2, 0);
boro 0:4beb2ea291ec 160 IRSensor irSensor1(distance, bit0, bit1, bit2, 1);
boro 0:4beb2ea291ec 161 IRSensor irSensor2(distance, bit0, bit1, bit2, 2);
boro 0:4beb2ea291ec 162 IRSensor irSensor3(distance, bit0, bit1, bit2, 3);
boro 0:4beb2ea291ec 163 IRSensor irSensor4(distance, bit0, bit1, bit2, 4);
boro 0:4beb2ea291ec 164 IRSensor irSensor5(distance, bit0, bit1, bit2, 5);
boro 0:4beb2ea291ec 165
boro 0:4beb2ea291ec 166 // enter main loop
boro 0:4beb2ea291ec 167
boro 0:4beb2ea291ec 168 enable_motors = 1;
boro 0:4beb2ea291ec 169 pwmLeft.period(0.00005f);// 0.05ms 20KHz
boro 0:4beb2ea291ec 170 pwmLeft.write(0.5f);
boro 0:4beb2ea291ec 171 pwmRight.period(0.00005f);// 0.05ms 20KHz
boro 0:4beb2ea291ec 172 pwmRight.write(0.5f);
boro 0:4beb2ea291ec 173 pwmReserve.period(0.00005f);// 0.05ms 20KHz
boro 0:4beb2ea291ec 174 pwmReserve.write(0.5f);
boro 0:4beb2ea291ec 175
boro 0:4beb2ea291ec 176
boro 0:4beb2ea291ec 177 /*
boro 0:4beb2ea291ec 178 sd.init();
boro 0:4beb2ea291ec 179 fs.mount(&sd);
boro 0:4beb2ea291ec 180 mkdir("/sd/Measure", 0777);
boro 0:4beb2ea291ec 181 printf("Folder created, SD initialisation done\r\n");
boro 0:4beb2ea291ec 182 FILE *fp = fopen("/sd/Measure/sdBlockDeviceTest.txt", "w");
boro 0:4beb2ea291ec 183 fprintf(fp, "accelerometer %.5f %.5f %.5f\r\n",imu.readAccelerationX(),imu.readAccelerationY(),imu.readAccelerationZ());
boro 0:4beb2ea291ec 184 fclose(fp);
boro 0:4beb2ea291ec 185 // read from SD card
boro 0:4beb2ea291ec 186 fp = fopen("/sd/Measure/sdBlockDeviceTest.txt", "r");
boro 0:4beb2ea291ec 187 if (fp != NULL) {
boro 0:4beb2ea291ec 188 char c = fgetc(fp);
boro 0:4beb2ea291ec 189 if (c == 'a')
boro 0:4beb2ea291ec 190 printf("SD write/read success!\r\n");
boro 0:4beb2ea291ec 191 else
boro 0:4beb2ea291ec 192 printf("incorrect char (%c)!\n", c);
boro 0:4beb2ea291ec 193 fclose(fp);
boro 0:4beb2ea291ec 194 } else {
boro 0:4beb2ea291ec 195 printf("Reading failed!\n");
boro 0:4beb2ea291ec 196 }*/
boro 0:4beb2ea291ec 197
boro 0:4beb2ea291ec 198 //pwmLeft.write(0.99f);
boro 0:4beb2ea291ec 199
boro 0:4beb2ea291ec 200 printf("\r\ninit done\r\n");
boro 0:4beb2ea291ec 201
boro 0:4beb2ea291ec 202 short previousValueCounterLeft = counterLeft.read();
boro 0:4beb2ea291ec 203 short previousValueCounterRight = counterRight.read();
boro 0:4beb2ea291ec 204
boro 0:4beb2ea291ec 205 while (true) {
boro 0:4beb2ea291ec 206
boro 0:4beb2ea291ec 207 led = !led;
boro 0:4beb2ea291ec 208
boro 0:4beb2ea291ec 209 //printf("ax = %.3f ay = %.3f az = %.3f\r\n",imu.readAccelerationX(),imu.readAccelerationY(),imu.readAccelerationZ());
boro 0:4beb2ea291ec 210
boro 0:4beb2ea291ec 211 //printf("counter right = %d counter left = %d counter res = %d\r\n",counterRight.read(),counterLeft.read(),counterRes.read());
boro 0:4beb2ea291ec 212 //printf("counter left = %.5f counter right = %.5f counter res = %.5f [uhmdreungen]\r\n",counterRight.read()/21504.0f,counterLeft.read()/21504.0f,counterRes.read()/21504.0f);
boro 0:4beb2ea291ec 213 //printf("din1 = %d din2 = %d din3 = %d\r\n",din1.read(),din2.read(),din3.read());
boro 0:4beb2ea291ec 214 //printf("ain1 = %.3f ain2 = %.3f ain3 = %.3f\r\n",ain1.read()*3.3f,ain2.read()*3.3f,ain3.read()*3.3f);
boro 0:4beb2ea291ec 215 //printf("%.3f | %.3f | %.3f\r\n %.3f | %.3f | %.3f\r\n\r\n",irSensor2.read(),irSensor0.read(),irSensor5.read(),irSensor3.read(),irSensor1.read(),irSensor4.read());
boro 0:4beb2ea291ec 216 //printf("angle right %.3f | angle left %.3f\r\n",controller.getangleRight(), controller.getangleLeft());
boro 0:4beb2ea291ec 217
boro 0:4beb2ea291ec 218 //printf("servo1 = %.3f servo2 = %.3f\r\n",servo1.read(),servo2.read());
boro 0:4beb2ea291ec 219
boro 0:4beb2ea291ec 220 printf("distance 0°=%d 90°=%d 180°=%d\r\n",lidar.getDistance(0),lidar.getDistance(90),lidar.getDistance(180));
boro 0:4beb2ea291ec 221
boro 0:4beb2ea291ec 222 if(user_button){
boro 0:4beb2ea291ec 223 //pwmLeft.write(0.5f);
boro 0:4beb2ea291ec 224 //pwmRight.write(0.5f);
boro 0:4beb2ea291ec 225 //pwmReserve.write(0.5f);
boro 0:4beb2ea291ec 226 //servo1.write(0.09f);
boro 0:4beb2ea291ec 227 //servo2.write(0.09f);
boro 0:4beb2ea291ec 228
boro 0:4beb2ea291ec 229 //uartGroove.printf("ciao\r\n");
boro 0:4beb2ea291ec 230 //i2c.write(0x48, 0x00, 1);
boro 0:4beb2ea291ec 231 //controller.setTranslationalVelocity(2.0f);
boro 0:4beb2ea291ec 232 //controller.startMeasure();
boro 0:4beb2ea291ec 233
boro 0:4beb2ea291ec 234 } else {
boro 0:4beb2ea291ec 235 //pwmLeft.write(0.99f);
boro 0:4beb2ea291ec 236 //pwmRight.write(0.99f);
boro 0:4beb2ea291ec 237 //pwmReserve.write(0.99f);
boro 0:4beb2ea291ec 238 //servo1.write(0.1f);
boro 0:4beb2ea291ec 239 //servo2.write(0.1f);
boro 0:4beb2ea291ec 240 //controller.setTranslationalVelocity(0.0f);
boro 0:4beb2ea291ec 241 }
boro 0:4beb2ea291ec 242
boro 0:4beb2ea291ec 243
boro 0:4beb2ea291ec 244
boro 0:4beb2ea291ec 245 float DISTANCE_THRESHOLD = 0.2f;
boro 0:4beb2ea291ec 246
boro 0:4beb2ea291ec 247 led0 = irSensor0 < DISTANCE_THRESHOLD;
boro 0:4beb2ea291ec 248 led1 = irSensor1 < DISTANCE_THRESHOLD;
boro 0:4beb2ea291ec 249 led2 = irSensor2 < DISTANCE_THRESHOLD;
boro 0:4beb2ea291ec 250 led3 = irSensor3 < DISTANCE_THRESHOLD;
boro 0:4beb2ea291ec 251 led4 = irSensor4 < DISTANCE_THRESHOLD;
boro 0:4beb2ea291ec 252 led5 = irSensor5 < DISTANCE_THRESHOLD;
boro 0:4beb2ea291ec 253
boro 0:4beb2ea291ec 254
boro 0:4beb2ea291ec 255 /*
boro 0:4beb2ea291ec 256 led1 = 0;
boro 0:4beb2ea291ec 257 led3 = 0;
boro 0:4beb2ea291ec 258 led4 = 0;
boro 0:4beb2ea291ec 259 led5 = 0;
boro 0:4beb2ea291ec 260 led6 = 0;*/
boro 0:4beb2ea291ec 261
boro 0:4beb2ea291ec 262
boro 0:4beb2ea291ec 263
boro 0:4beb2ea291ec 264 /*
boro 0:4beb2ea291ec 265 led1 = !led1;
boro 0:4beb2ea291ec 266 led2 = !led2;
boro 0:4beb2ea291ec 267 led3 = !led3;
boro 0:4beb2ea291ec 268 led4 = !led4;
boro 0:4beb2ea291ec 269 led5 = !led5;
boro 0:4beb2ea291ec 270 led6 = !led6;*/
boro 0:4beb2ea291ec 271
boro 0:4beb2ea291ec 272 /*
boro 0:4beb2ea291ec 273
boro 0:4beb2ea291ec 274 short valueCounterLeft = counterLeft.read();
boro 0:4beb2ea291ec 275 short valueCounterRight = counterRight.read();
boro 0:4beb2ea291ec 276
boro 0:4beb2ea291ec 277 short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
boro 0:4beb2ea291ec 278 short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
boro 0:4beb2ea291ec 279
boro 0:4beb2ea291ec 280 previousValueCounterLeft = valueCounterLeft;
boro 0:4beb2ea291ec 281 previousValueCounterRight = valueCounterRight;
boro 0:4beb2ea291ec 282
boro 0:4beb2ea291ec 283 countsRight = countsRight + countsInPastPeriodRight;
boro 0:4beb2ea291ec 284 countsLeft = countsLeft + countsInPastPeriodLeft;
boro 0:4beb2ea291ec 285
boro 0:4beb2ea291ec 286 printf("counter right = %d counter left = %d\r\n",countsRight,countsLeft);*/
boro 0:4beb2ea291ec 287
boro 0:4beb2ea291ec 288 /*
boro 0:4beb2ea291ec 289 FILE *fp = fopen("/sd/Measure/sdBlockDeviceTest.txt", "a");
boro 0:4beb2ea291ec 290 fprintf(fp, "accelerometer %.5f %.5f %.5f\r\n",imu.readAccelerationX(),imu.readAccelerationY(),imu.readAccelerationZ());
boro 0:4beb2ea291ec 291 fclose(fp);*/
boro 0:4beb2ea291ec 292
boro 0:4beb2ea291ec 293
boro 0:4beb2ea291ec 294
boro 0:4beb2ea291ec 295 Thread::wait(200);
boro 0:4beb2ea291ec 296 }
boro 0:4beb2ea291ec 297 }
boro 0:4beb2ea291ec 298
boro 0:4beb2ea291ec 299 /**
boro 0:4beb2ea291ec 300 * This main method is the entry point of the controller software.
boro 0:4beb2ea291ec 301 * The code in this method is running within a thread of the rtos.
boro 0:4beb2ea291ec 302 */
boro 0:4beb2ea291ec 303 int main() {
boro 0:4beb2ea291ec 304
boro 0:4beb2ea291ec 305 Main main;
boro 0:4beb2ea291ec 306
boro 0:4beb2ea291ec 307 Thread::wait(osWaitForever);
boro 0:4beb2ea291ec 308 }
boro 0:4beb2ea291ec 309