Main.cpp

00001 /*
00002  * Main.cpp
00003  * Copyright (c) 2017, ZHAW
00004  * All rights reserved.
00005  *
00006  *  Created on: 07.07.2017
00007  *      Author: Marcel Honegger
00008  */
00009 
00010 #include <mbed.h>
00011 #include <EthernetInterface.h>
00012 //#include "SDFileSystem/SDFileSystem.h"
00013 #include "HTTPServer.h"
00014 #include "IndexScript.h"
00015 #include "ControllerScript.h"
00016 #include "EncoderCounter.h"
00017 #include "IMU.h"
00018 #include "IRSensor.h"
00019 #include "SDBlockDevice.h"
00020 #include "FATFileSystem.h"
00021 #include "IRSensor.h"
00022 #include "Controller.h"
00023 #include "LIDAR.h"
00024 
00025 class Main {
00026     
00027     public:
00028         
00029                 Main();
00030         void    run();
00031         
00032     private:
00033         
00034         Thread  thread;
00035 };
00036 
00037 Main::Main() : thread(osPriorityNormal, 64*1024) {
00038     
00039     thread.start(callback(this, &Main::run));
00040 }
00041 
00042 void Main::run() {
00043      
00044     // create user_button object
00045     DigitalIn user_button(USER_BUTTON);
00046     DigitalOut led(LED1);
00047     DigitalOut myLed2(LED2);
00048     
00049     DigitalOut enableRouter(PB_15);
00050     enableRouter = 1;
00051     
00052     printf("Starting Router\r\n");
00053     Thread::wait(1000);
00054     printf("Starting webserver\r\n");
00055     
00056     // create webserver
00057     EthernetInterface* ethernet = new EthernetInterface();
00058     ethernet->set_network("192.168.0.100", "255.255.255.0", "192.168.1.1");
00059     ethernet->connect();
00060     printf("The target IP address is '%s'\r\n", ethernet->get_ip_address());
00061     
00062     HTTPServer httpServer(*ethernet);
00063     
00064         
00065     
00066     
00067     // initialise PWM objects
00068     PwmOut pwmRight(PF_8);
00069     PwmOut pwmLeft(PF_7);
00070     PwmOut pwmReserve(PF_9);
00071     
00072     // crete Encoder read objects
00073     EncoderCounter counterRight(PB_4, PC_7); // counter(pin A, pin B)
00074     EncoderCounter counterLeft(PA_15, PB_3);
00075     EncoderCounter counterRes(PD_12, PD_13);
00076     
00077     PwmOut servo1(PE_10);
00078     PwmOut servo2(PE_12);
00079     
00080     servo1.period(0.02f);// 0.02ms 50Hz
00081     servo2.period(0.02f);// 0.02ms 50Hz
00082     servo1.write(0.01f);
00083     servo2.write(0.01f);
00084     
00085     Controller controller(pwmReserve, pwmRight, counterRes, counterRight);
00086     
00087     // create LIDAR object
00088     PwmOut pwm(PE_9);
00089     pwm.period(0.00005f);
00090     pwm.write(0.4f);      // 50% duty-cycle
00091     
00092     RawSerial com(PG_14, PG_9);
00093     
00094     LIDAR lidar(com);
00095     
00096     httpServer.add("index", new IndexScript());
00097     httpServer.add("controller", new ControllerScript(user_button,myLed2,controller,servo1,servo2,lidar));
00098     
00099     // create IMU comunication objects
00100     SPI spi(PC_12, PC_11, PC_10); // mosi, miso, sclk
00101     DigitalOut csAG(PC_8);
00102     DigitalOut csM(PC_9);
00103     
00104     //create IMU object
00105     IMU imu(spi, csAG, csM);
00106     printf("IMU init done\r\n");
00107     // create IRSensor object
00108     AnalogIn distance(PA_0);
00109     
00110     // create multiplexer objects
00111     DigitalOut bit0(PF_0);
00112     DigitalOut bit1(PF_1);
00113     DigitalOut bit2(PF_2);
00114     
00115     // create sd card object
00116     SDBlockDevice sd(PE_6, PE_5, PE_2, PE_4);
00117     FATFileSystem fs("fs", &sd); 
00118     printf("SD initialisation done\r\n");
00119     
00120     // others
00121     DigitalOut enable_motors(PG_0);
00122     DigitalIn driver_warning(PD_0);
00123     DigitalIn driver_fault(PD_1);
00124     
00125     DigitalOut enable_IRsens(PG_1);
00126     
00127     CAN can(PB_12, PB_6);
00128     
00129     AnalogIn ain1(PB_1);
00130     AnalogIn ain2(PC_2);
00131     AnalogIn ain3(PF_4);
00132     
00133     DigitalIn din1(PE_14);
00134     DigitalIn din2(PE_15);
00135     DigitalIn din3(PE_0);
00136     
00137     I2C i2c(PB_11,PB_10);
00138     
00139     
00140     
00141     Serial uartGroove(PE_8, PE_7); // tx, rx
00142     
00143     DigitalOut led0(PD_4);
00144     DigitalOut led1(PD_3);
00145     DigitalOut led2(PD_6);
00146     DigitalOut led3(PD_2);
00147     DigitalOut led4(PD_7);
00148     DigitalOut led5(PD_5);
00149     
00150     led0 = 0;
00151     led1 = 0;
00152     led2 = 0;
00153     led3 = 0;
00154     led4 = 0;
00155     led5 = 0;
00156     
00157     enable_IRsens = 1;
00158     
00159     IRSensor irSensor0(distance, bit0, bit1, bit2, 0);
00160     IRSensor irSensor1(distance, bit0, bit1, bit2, 1);
00161     IRSensor irSensor2(distance, bit0, bit1, bit2, 2);
00162     IRSensor irSensor3(distance, bit0, bit1, bit2, 3);
00163     IRSensor irSensor4(distance, bit0, bit1, bit2, 4);
00164     IRSensor irSensor5(distance, bit0, bit1, bit2, 5);
00165 
00166     // enter main loop
00167     
00168     enable_motors = 1;
00169     pwmLeft.period(0.00005f);// 0.05ms 20KHz
00170     pwmLeft.write(0.5f);
00171     pwmRight.period(0.00005f);// 0.05ms 20KHz
00172     pwmRight.write(0.5f);
00173     pwmReserve.period(0.00005f);// 0.05ms 20KHz
00174     pwmReserve.write(0.5f);
00175     
00176     printf("Test writing... ");
00177     FILE* fp = fopen("/fs/data.csv", "w");
00178     fprintf(fp, "test %.5f\r\n",1.23);
00179     fclose(fp);
00180     printf("done\r\n");
00181     
00182     printf("Test reading... ");
00183     // read from SD card
00184     fp = fopen("/fs/data.csv", "r");
00185     if (fp != NULL) {
00186         char c = fgetc(fp);
00187         if (c == 't')
00188             printf("done\r\n");
00189         else
00190             printf("incorrect char (%c)!\n", c);
00191         fclose(fp);
00192     } else {
00193         printf("Reading failed!\n");
00194     }
00195 
00196     
00197     
00198     printf("\r\ninit done\r\n");
00199     
00200     short previousValueCounterLeft = counterLeft.read();
00201     short previousValueCounterRight = counterRight.read();
00202     
00203     while (true) {
00204         
00205         led = !led;
00206         
00207         printf("ax = %.3f ay = %.3f az = %.3f\r\n",imu.readAccelerationX(),imu.readAccelerationY(),imu.readAccelerationZ());
00208         
00209         //printf("counter right = %d counter left = %d counter res = %d\r\n",counterRight.read(),counterLeft.read(),counterRes.read());
00210         //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);
00211         //printf("din1 = %d din2 = %d din3 = %d\r\n",din1.read(),din2.read(),din3.read());
00212         //printf("ain1 = %.3f ain2 = %.3f ain3 = %.3f\r\n",ain1.read()*3.3f,ain2.read()*3.3f,ain3.read()*3.3f);
00213         //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());
00214         //printf("angle right %.3f | angle left %.3f\r\n",controller.getangleRight(), controller.getangleLeft());
00215         
00216         //printf("servo1 = %.3f servo2 = %.3f\r\n",servo1.read(),servo2.read());
00217         
00218         //printf("distance 0=%d 90=%d 180=%d\r\n",static_cast<int16_t>(lidar.getDistance(0)),static_cast<int16_t>(lidar.getDistance(90)),static_cast<int16_t>(lidar.getDistance(180)));
00219         
00220         if(user_button){
00221             //pwmLeft.write(0.5f);
00222             //pwmRight.write(0.5f);
00223             //pwmReserve.write(0.5f);
00224             //servo1.write(0.09f);
00225             //servo2.write(0.09f);
00226             
00227             //uartGroove.printf("ciao\r\n");
00228             //i2c.write(0x48, 0x00, 1);
00229             //controller.setTranslationalVelocity(2.0f);
00230             //controller.startMeasure();
00231             
00232         } else {
00233             //pwmLeft.write(0.99f);
00234             //pwmRight.write(0.99f);
00235             //pwmReserve.write(0.99f);
00236             //servo1.write(0.1f);
00237             //servo2.write(0.1f);
00238             //controller.setTranslationalVelocity(0.0f);
00239         }
00240         
00241         
00242         // The LED turn on if an obstacle is closer than 20cm
00243         float DISTANCE_THRESHOLD = 0.2f;
00244         led0 = irSensor0 < DISTANCE_THRESHOLD;
00245         led1 = irSensor1 < DISTANCE_THRESHOLD;
00246         led2 = irSensor2 < DISTANCE_THRESHOLD;
00247         led3 = irSensor3 < DISTANCE_THRESHOLD;
00248         led4 = irSensor4 < DISTANCE_THRESHOLD;
00249         led5 = irSensor5 < DISTANCE_THRESHOLD;
00250         
00251             
00252         Thread::wait(500);
00253     }
00254 }
00255 
00256 /**
00257  * This main method is the entry point of the controller software.
00258  * The code in this method is running within a thread of the rtos.
00259  */
00260 int main() {
00261     
00262     Main main;
00263     
00264     Thread::wait(osWaitForever);
00265 }
00266