Diff: main.cpp

Revision:

1:ae213d6a0adf

Parent:

0:caffc01f54aa

--- a/main.cpp	Sat Jan 26 12:26:19 2013 +0000
+++ b/main.cpp	Sat May 14 21:15:12 2016 +0000
@@ -1,29 +1,152 @@
 #include "mbed.h"
+#include "stdio.h"
+#include "quad_core.h"
+#include "bldc.h"
 #include "cmsis_os.h"
+#include "Mutex.h"
+#include "hcsr04.h"
+#include "rtos.h"
+using namespace rtos;
 
-DigitalOut led1(LED1);
-DigitalOut led2(LED2);
+DigitalOut myled(LED1);
+DigitalOut myled2(LED2);
+//Serial pc
 Serial pc(USBTX , USBRX);
+
+// Init i2c
+I2C i2c(p28, p27);
+
+// Bluetooth
+Serial blue(p9, p10);
+
+//Senzori ultrasonici
+HCSR04  leftsensor(p25,p26);
+HCSR04  centersensor(p21,p22);
+HCSR04  rightsensor(p23,p24);
+
+//Initializare clasa Mutex
+Mutex m;
+//slave adress
+
+// slave address
+//Slave 1 adress
+const int slave1 = 0x20;    // 7 bits 0x10
+//Slave 2 address
+const int slave2 = 0x22;    // 7 bits  0x11
+
+//char cmd[10];
+//unsigned short duty = 15;                // 0 - 150
+//unsigned short freq = 11000;            // 11 kHz
+//char direction = 0;                     // nu intereseaza
+//unsigned short speed = 20;               // freq comutatie intre faze
+
+bldc motorLeft(&i2c, slave1);
+bldc motorRight(&i2c, slave2);
+
+//Distante senzori
+unsigned int dists, distd, distc;
+
+volatile int race = 0;
+volatile int test = 0;
+
+
+void race_thread(void const *args)
+{
+    while (race ) {
+        myled = !myled;
+        wait(0.4);
+    }
+    myled = 0;
     
-void led2_thread(void const *args){
-    while (true) {
-        led2 = !led2;
-        osDelay(1000);
+    
+}
+
+void test_thread(void const *args)
+{
+    while(test) {
+        myled2 = !myled2;
+        wait(0.2);
+    }
+    myled2=0;
+}
+
+
+void sensor_thread(const void *args)
+{
+
+    while(true) {
+
+        leftsensor.start();
+        rightsensor.start();
+        centersensor.start();
+
+        dists=leftsensor.get_dist_cm();
+        distd=rightsensor.get_dist_cm();
+        distc=centersensor.get_dist_cm();
+
+
+
     }
 }
 
-void serial_thread(const void *args){
-    pc.baud(115200);
-    while(true){
-        pc.putc(pc.getc());
+osThreadDef(sensor_thread, osPriorityNormal, DEFAULT_STACK_SIZE);
+osThreadDef(race_thread, osPriorityNormal, DEFAULT_STACK_SIZE);
+osThreadDef(test_thread, osPriorityNormal, DEFAULT_STACK_SIZE);
+
+int main()
+{
+    time_t start = time(NULL);
+
+
+    //Setup Frequency
+    pc.baud(9600);
+    blue.baud(9600);
+    i2c.frequency(8000000);
+    //Crearea unui thread
+    Thread t1(sensor_thread, (void *)"Th sensor");
+
+    while(1) {
+        time_t seconds = time(NULL) - start;
+        wait(1);
+        //pc.printf("%d-%d-%d-%d\n\r", dists, distc, distd, seconds);
+        blue.printf("%d-%d-%d-%d\n", dists, distc, distd, seconds);
+        wait(0.2);
+
+        if (blue.readable()) {
+            char code = blue.getc() - 48;
+            char byte1 = blue.getc();
+            char byte2 = blue.getc();
+//            write_message(code, byte1, byte2);
+            switch(code) {
+                case CODE_DUTY_RIGHT:
+                    motorRight.duty(byte1, byte2);
+                    break;
+                case CODE_DUTY_LEFT :
+                    motorLeft.duty(byte1, byte2);
+                    break;
+                case CODE_DUTY:
+                    motorRight.duty(byte1, byte2);
+                    motorLeft.duty(byte1, byte2);
+                    break;
+            }
+            // pc.printf("%d %d", byte1, byte2);
+
+            int key = blue.getc();
+            //Race Mode
+            if(key == START_RACE_MODE) {
+
+                osThreadCreate(osThread(race_thread),NULL);
+                race = 1;
+                test=0;
+
+            } else if (key== START_TEST_MODE) {
+                race=0;
+                osThreadCreate(osThread(test_thread),NULL);
+                test=1;
+
+
+            }
+
+        }
     }
 }
-
-osThreadDef(serial_thread, osPriorityNormal, DEFAULT_STACK_SIZE);
-osThreadDef(led2_thread, osPriorityNormal, DEFAULT_STACK_SIZE);
-
-int main() {
-    osThreadCreate(osThread(led2_thread), NULL);
-    osThreadCreate(osThread(serial_thread), NULL);
-    while(true);
-}