Revision 1:ae213d6a0adf, committed 2016-05-14

Comitter:

machines94

Date:

Sat May 14 21:15:12 2016 +0000

Parent:

0:caffc01f54aa

Commit message:

Electromobility 2016
;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/bldc/bldc.cpp	Sat May 14 21:15:12 2016 +0000
@@ -0,0 +1,33 @@
+#include "bldc.h"
+#include "quad_core.h"
+bldc::bldc(I2C* i2c, int addr) {
+    this->_i2c = new i2c_helper(i2c, addr);
+    init_cmd();
+}
+
+void bldc::duty(char dutyMSB, char dutyLSB) {
+    this->_i2c->write_message( CODE_DUTY, dutyMSB,  dutyLSB, this->cmd);
+}
+
+void bldc::init_cmd() {
+    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
+    
+    cmd[0] = speed >> 8;
+    cmd[1] = speed;
+    
+    cmd[2] = (char) duty >> 8;
+    cmd[3] = (char) duty;
+
+    cmd[4] = freq >>8;
+    cmd[5] = freq;
+    
+    cmd[6] = direction;
+}
+
+bldc::~bldc() {
+    delete this->_i2c;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/bldc/bldc.h	Sat May 14 21:15:12 2016 +0000
@@ -0,0 +1,18 @@
+#ifndef BLDC_H
+#define BLDC_H
+
+#include "mbed.h"
+#include "i2c_helper.h"
+
+class bldc {
+    public:
+        bldc(I2C* i2c, int addr);
+        void bldc::duty(char dutyMSB, char dutyLSB);
+        ~bldc();
+    private:
+        void init_cmd();
+        i2c_helper* _i2c; 
+        char cmd[10];
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/bldc/i2c_helper.cpp	Sat May 14 21:15:12 2016 +0000
@@ -0,0 +1,31 @@
+#include "i2c_helper.h"
+
+unsigned short duty = 15;                // 0 - 150
+unsigned short freq = 11000;            // 11 kHz - pwm frequecny
+char direction = 0;                     // motor direction
+unsigned short speed = 20;               // bldc phase comutation freq 
+
+i2c_helper::i2c_helper(I2C* i2c, int addr){
+    this->addr = addr;
+    this->i2c = i2c;
+}
+
+int i2c_helper::write_message(char code, char byte1, char byte2, char cmd[]) {
+    if( code == 1) {
+        duty = (unsigned short)(byte1 << 8) + byte2;
+        cmd[2] = (char) duty >> 8;
+        cmd[3] = (char) duty;
+    } else if ( code == 0 ) {
+        speed = (unsigned short)(byte1 << 8) + byte2;
+    }
+
+    cmd[0] = speed >> 8;
+    cmd[1] = speed;
+
+
+    cmd[4] = freq >>8;
+    cmd[5] = freq;
+
+    cmd[6] = direction;
+    return this->i2c->write(this->addr, cmd, 8);
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/bldc/i2c_helper.h	Sat May 14 21:15:12 2016 +0000
@@ -0,0 +1,15 @@
+#ifndef I2C_HELPER_H
+#define I2C_HELPER_H
+
+#include "mbed.h"
+
+class i2c_helper{
+    public:
+        i2c_helper(I2C* i2c, int addr);
+        int write_message(char code, char byte1, char byte2, char cmd[]);
+    
+    private:
+        int addr;
+        I2C* i2c;
+};
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/hcsr04/hcsr04.cpp	Sat May 14 21:15:12 2016 +0000
@@ -0,0 +1,98 @@
+/* Copyright (c) 2013 Prabhu Desai
+ * pdtechworld@gmail.com
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+ * and associated documentation files (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge, publish, distribute,
+ * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all copies or
+ * substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+ * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+
+#include "hcsr04.h"
+
+
+HCSR04::HCSR04(PinName TrigPin,PinName EchoPin):
+    trigger(TrigPin), echo(EchoPin)
+{
+    pulsetime.stop();
+    pulsetime.reset();
+    echo.rise(this,&HCSR04::isr_rise);
+    echo.fall(this,&HCSR04::isr_fall);
+    trigger=0;
+}
+
+HCSR04::~HCSR04()
+{
+}
+
+void HCSR04::isr_rise(void)
+{
+    pulsetime.start();
+}
+void HCSR04::start(void)
+{
+    trigger=1;
+    wait_us(10);
+    trigger=0;
+}
+
+void HCSR04::isr_fall(void)
+{
+    pulsetime.stop();
+    pulsedur = pulsetime.read_us();
+    distance= (pulsedur*343)/20000;
+    pulsetime.reset();
+}
+
+void HCSR04::rise (void (*fptr)(void))
+{
+    echo.rise(fptr);
+}
+void HCSR04::fall (void (*fptr)(void))
+{
+    echo.fall(fptr);
+}
+
+unsigned int HCSR04::get_dist_cm()
+{
+    return distance;
+}
+unsigned int HCSR04::get_pulse_us()
+{
+    return pulsedur;
+}
+
+
+
+/*******************************************************
+   Here is a sample code usage
+********************************************************* 
+#include "hcsr04.h"
+HCSR04  usensor(p25,p6);
+int main()
+{
+    unsigned char count=0;
+    while(1) {
+        usensor.start();
+        wait_ms(500); 
+        dist=usensor.get_dist_cm();
+        lcd.cls();
+        lcd.locate(0,0);
+        lcd.printf("cm:%ld",dist );
+ 
+        count++;
+        lcd.locate(0,1);
+        lcd.printf("Distance =%d",count);
+        
+    }
+*/
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/hcsr04/hcsr04.h	Sat May 14 21:15:12 2016 +0000
@@ -0,0 +1,68 @@
+/* Copyright (c) 2013 Prabhu Desai
+ * pdtechworld@gmail.com
+ *
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+ * and associated documentation files (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge, publish, distribute,
+ * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all copies or
+ * substantial portions of the Software.
+ *
+ * For more details on the sensor :
+ * http://www.elecfreaks.com/store/hcsr04-ultrasonic-sensor-distance-measuring-module-p-91.html?zenid=pgm8pgnvaodbe36dibq5s1soi3
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+ * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#ifndef MBED_HCSR04_H
+#define MBED_HCSR04_H
+
+#include "mbed.h"
+
+/** HCSR04 Class(es)
+ */
+
+class HCSR04
+{
+public:
+    /** Create a HCSR04 object connected to the specified pin
+    * @param pin i/o pin to connect to
+    */
+    HCSR04(PinName TrigPin,PinName EchoPin);
+    ~HCSR04();
+
+    /** Return the distance from obstacle in cm
+    * @param distance in cms and returns -1, in case of failure
+    */
+    unsigned int get_dist_cm(void);
+    /** Return the pulse duration equal to sonic waves travelling to obstacle and back to receiver.
+    * @param pulse duration in microseconds.
+    */
+    unsigned int get_pulse_us(void);
+    /** Generates the trigger pulse of 10us on the trigger PIN.
+    */
+    void start(void );
+    void isr_rise(void);
+    void isr_fall(void);
+    void fall (void (*fptr)(void));
+    void rise (void (*fptr)(void));
+
+
+
+private:
+
+    Timer pulsetime;
+    DigitalOut  trigger;
+    InterruptIn echo;
+    unsigned int pulsedur;
+    unsigned int distance;
+};
+
+#endif
\ No newline at end of file

--- 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);
-}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/quad_core.h	Sat May 14 21:15:12 2016 +0000
@@ -0,0 +1,13 @@
+#ifndef _QUAD_CORE_H
+#define _QUAD_CORE_H
+
+#define START_RACE_MODE 'p'
+#define START_TEST_MODE 'd'
+
+#define COMMUT_FREQUENCY_DUTY 0
+#define CODE_DUTY   1
+#define CODE_DUTY_RIGHT 2
+#define CODE_DUTY_LEFT 3
+
+#define DISTANCE 15
+#endif
\ No newline at end of file