Diff: Main.cpp

Revision:

0:4beb2ea291ec

Child:

1:94c549ec20f9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Main.cpp	Mon Mar 16 13:12:31 2020 +0000
@@ -0,0 +1,309 @@
+/*
+ * Main.cpp
+ * Copyright (c) 2017, ZHAW
+ * All rights reserved.
+ *
+ *  Created on: 07.07.2017
+ *      Author: Marcel Honegger
+ */
+
+#include <mbed.h>
+#include <EthernetInterface.h>
+//#include "SDFileSystem/SDFileSystem.h"
+#include "HTTPServer.h"
+#include "IndexScript.h"
+#include "ControllerScript.h"
+#include "EncoderCounter.h"
+#include "IMU.h"
+#include "IRSensor.h"
+#include "SDBlockDevice.h"
+#include "FATFileSystem.h"
+#include "IRSensor.h"
+#include "Controller.h"
+#include "LIDAR.h"
+
+class Main {
+    
+    public:
+        
+                Main();
+        void    run();
+        
+    private:
+        
+        Thread  thread;
+};
+
+Main::Main() : thread(osPriorityNormal, 64*1024) {
+    
+    thread.start(callback(this, &Main::run));
+}
+
+void Main::run() {
+     
+    // create user_button object
+    DigitalIn user_button(USER_BUTTON);
+    DigitalOut led(LED1);
+    DigitalOut myLed2(LED2);
+    
+    DigitalOut enableRouter(PB_15);
+    enableRouter = 1;
+    
+    printf("Starting Router\r\n");
+    Thread::wait(1000);
+    printf("Starting webserver\r\n");
+    
+    // create webserver
+    EthernetInterface* ethernet = new EthernetInterface();
+    ethernet->set_network("192.168.0.100", "255.255.255.0", "192.168.1.1");
+    ethernet->connect();
+    printf("The target IP address is '%s'\r\n", ethernet->get_ip_address());
+    
+    HTTPServer httpServer(*ethernet);
+    
+        
+    
+    
+    // initialise PWM objects
+    PwmOut pwmRight(PF_8);
+    PwmOut pwmLeft(PF_7);
+    PwmOut pwmReserve(PF_9);
+    
+    // crete Encoder read objects
+    EncoderCounter counterRight(PB_4, PC_7); // counter(pin A, pin B)
+    EncoderCounter counterLeft(PA_15, PB_3);
+    EncoderCounter counterRes(PD_12, PD_13);
+    
+    PwmOut servo1(PE_10);
+    PwmOut servo2(PE_12);
+    
+    servo1.period(0.02f);// 0.02ms 50Hz
+    servo2.period(0.02f);// 0.02ms 50Hz
+    servo1.write(0.01f);
+    servo2.write(0.01f);
+    
+    Controller controller(pwmReserve, pwmRight, counterRes, counterRight);
+    
+    // create LIDAR object
+    PwmOut pwm(PE_9);
+    pwm.period(0.00005f);
+    pwm.write(0.5f);      // 50% duty-cycle
+    
+    RawSerial com(PG_14, PG_9);
+    
+    LIDAR lidar(com);
+    
+    httpServer.add("index", new IndexScript());
+    httpServer.add("controller", new ControllerScript(user_button,myLed2,controller,servo1,servo2,lidar));
+    
+    // create IMU comunication objects
+    SPI spi(PC_12, PC_11, PC_10); // mosi, miso, sclk
+    DigitalOut csAG(PC_8);
+    DigitalOut csM(PC_9);
+    
+    //create IMU object
+    IMU imu(spi, csAG, csM);
+    printf("IMU init done\r\n");
+    // create IRSensor object
+    AnalogIn distance(PA_0);
+    
+    // create multiplexer objects
+    DigitalOut bit0(PF_0);
+    DigitalOut bit1(PF_1);
+    DigitalOut bit2(PF_2);
+    
+    // create sd card object
+    SDBlockDevice sd(PE_6, PE_5, PE_2, PE_4); // mosi miso clk cs
+    FATFileSystem fs("sd");
+    printf("SD FileSystem init done\r\n");
+    
+    // others
+    DigitalOut enable_motors(PG_0);
+    DigitalIn driver_warning(PD_0);
+    DigitalIn driver_fault(PD_1);
+    
+    DigitalOut enable_IRsens(PG_1);
+    
+    CAN can(PB_12, PB_6);
+    
+    AnalogIn ain1(PB_1);
+    AnalogIn ain2(PC_2);
+    AnalogIn ain3(PF_4);
+    
+    DigitalIn din1(PE_14);
+    DigitalIn din2(PE_15);
+    DigitalIn din3(PE_0);
+    
+    I2C i2c(PB_11,PB_10);
+    
+    
+    
+    Serial uartGroove(PE_8, PE_7); // tx, rx
+    
+    DigitalOut led0(PD_4);
+    DigitalOut led1(PD_3);
+    DigitalOut led2(PD_6);
+    DigitalOut led3(PD_2);
+    DigitalOut led4(PD_7);
+    DigitalOut led5(PD_5);
+    
+    led0 = 0;
+    led1 = 0;
+    led2 = 0;
+    led3 = 0;
+    led4 = 0;
+    led5 = 0;
+    
+    enable_IRsens = 1;
+    
+    IRSensor irSensor0(distance, bit0, bit1, bit2, 0);
+    IRSensor irSensor1(distance, bit0, bit1, bit2, 1);
+    IRSensor irSensor2(distance, bit0, bit1, bit2, 2);
+    IRSensor irSensor3(distance, bit0, bit1, bit2, 3);
+    IRSensor irSensor4(distance, bit0, bit1, bit2, 4);
+    IRSensor irSensor5(distance, bit0, bit1, bit2, 5);
+
+    // enter main loop
+    
+    enable_motors = 1;
+    pwmLeft.period(0.00005f);// 0.05ms 20KHz
+    pwmLeft.write(0.5f);
+    pwmRight.period(0.00005f);// 0.05ms 20KHz
+    pwmRight.write(0.5f);
+    pwmReserve.period(0.00005f);// 0.05ms 20KHz
+    pwmReserve.write(0.5f);
+    
+    
+    /*
+    sd.init();
+    fs.mount(&sd);
+    mkdir("/sd/Measure", 0777);
+    printf("Folder created, SD initialisation done\r\n");
+    FILE *fp = fopen("/sd/Measure/sdBlockDeviceTest.txt", "w");
+    fprintf(fp, "accelerometer %.5f %.5f %.5f\r\n",imu.readAccelerationX(),imu.readAccelerationY(),imu.readAccelerationZ());
+    fclose(fp);
+    // read from SD card
+    fp = fopen("/sd/Measure/sdBlockDeviceTest.txt", "r");
+    if (fp != NULL) {
+        char c = fgetc(fp);
+        if (c == 'a')
+            printf("SD write/read success!\r\n");
+        else
+            printf("incorrect char (%c)!\n", c);
+        fclose(fp);
+    } else {
+        printf("Reading failed!\n");
+    }*/
+    
+    //pwmLeft.write(0.99f);
+    
+    printf("\r\ninit done\r\n");
+    
+    short previousValueCounterLeft = counterLeft.read();
+    short previousValueCounterRight = counterRight.read();
+    
+    while (true) {
+        
+        led = !led;
+        
+        //printf("ax = %.3f ay = %.3f az = %.3f\r\n",imu.readAccelerationX(),imu.readAccelerationY(),imu.readAccelerationZ());
+        
+        //printf("counter right = %d counter left = %d counter res = %d\r\n",counterRight.read(),counterLeft.read(),counterRes.read());
+        //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);
+        //printf("din1 = %d din2 = %d din3 = %d\r\n",din1.read(),din2.read(),din3.read());
+        //printf("ain1 = %.3f ain2 = %.3f ain3 = %.3f\r\n",ain1.read()*3.3f,ain2.read()*3.3f,ain3.read()*3.3f);
+        //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());
+        //printf("angle right %.3f | angle left %.3f\r\n",controller.getangleRight(), controller.getangleLeft());
+        
+        //printf("servo1 = %.3f servo2 = %.3f\r\n",servo1.read(),servo2.read());
+        
+        printf("distance 0°=%d 90°=%d 180°=%d\r\n",lidar.getDistance(0),lidar.getDistance(90),lidar.getDistance(180));
+        
+        if(user_button){
+            //pwmLeft.write(0.5f);
+            //pwmRight.write(0.5f);
+            //pwmReserve.write(0.5f);
+            //servo1.write(0.09f);
+            //servo2.write(0.09f);
+            
+            //uartGroove.printf("ciao\r\n");
+            //i2c.write(0x48, 0x00, 1);
+            //controller.setTranslationalVelocity(2.0f);
+            //controller.startMeasure();
+            
+        } else {
+            //pwmLeft.write(0.99f);
+            //pwmRight.write(0.99f);
+            //pwmReserve.write(0.99f);
+            //servo1.write(0.1f);
+            //servo2.write(0.1f);
+            //controller.setTranslationalVelocity(0.0f);
+        }
+        
+        
+        
+        float DISTANCE_THRESHOLD = 0.2f;
+        
+        led0 = irSensor0 < DISTANCE_THRESHOLD;
+        led1 = irSensor1 < DISTANCE_THRESHOLD;
+        led2 = irSensor2 < DISTANCE_THRESHOLD;
+        led3 = irSensor3 < DISTANCE_THRESHOLD;
+        led4 = irSensor4 < DISTANCE_THRESHOLD;
+        led5 = irSensor5 < DISTANCE_THRESHOLD;
+        
+        
+        /*
+        led1 = 0;
+        led3 = 0;
+        led4 = 0;
+        led5 = 0;
+        led6 = 0;*/
+        
+        
+        
+        /*
+        led1 = !led1;
+        led2 = !led2;
+        led3 = !led3;
+        led4 = !led4;
+        led5 = !led5;
+        led6 = !led6;*/
+        
+        /*
+        
+        short valueCounterLeft = counterLeft.read();
+        short valueCounterRight = counterRight.read();
+
+        short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
+        short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
+
+        previousValueCounterLeft = valueCounterLeft;
+        previousValueCounterRight = valueCounterRight;
+        
+        countsRight = countsRight + countsInPastPeriodRight;
+        countsLeft = countsLeft + countsInPastPeriodLeft;
+        
+        printf("counter right = %d counter left = %d\r\n",countsRight,countsLeft);*/
+        
+        /*
+        FILE *fp = fopen("/sd/Measure/sdBlockDeviceTest.txt", "a");
+        fprintf(fp, "accelerometer %.5f %.5f %.5f\r\n",imu.readAccelerationX(),imu.readAccelerationY(),imu.readAccelerationZ());
+        fclose(fp);*/
+        
+        
+            
+        Thread::wait(200);
+    }
+}
+
+/**
+ * This main method is the entry point of the controller software.
+ * The code in this method is running within a thread of the rtos.
+ */
+int main() {
+    
+    Main main;
+    
+    Thread::wait(osWaitForever);
+}
+