Diff: main.cpp

Revision:

3:bb074b1d26fe

Parent:

2:35f13b7f3659

Child:

4:4ea0f6305fa3

--- a/main.cpp	Thu Nov 23 13:09:25 2017 +0000
+++ b/main.cpp	Wed Jan 29 03:02:47 2020 +0000
@@ -1,31 +1,254 @@
 #include "mbed.h"
+#include "mavlink.h"
+#include "motordriver.h"
+#include "Map.hpp"
+#include "rtos.h"
+#include "IncrementalEncoder.h"
+#include "VL53L1X.h"
+
+#define FRONT 1
+#define NORMAL 0
+#define BACK 2
+#define LEFT 3
+#define RIGHT 4
+
+DigitalOut led (LED2);
+//SERIAL
+//Serial pc(USBTX, USBRX,115200);
+RawSerial xbee(PC_10, PC_11, 115200);
+
+//Thread
+Thread thread;
+Thread thread2;
+Mutex mutex;
+
+// OF MAVLINK
+char buf[MAVLINK_MAX_PACKET_LEN];
+
+mavlink_message_t message;
+mavlink_message_t r_message;
+mavlink_status_t status;
+mavlink_status_t r_status;
+
+mavlink_heartbeat_t heartbeat;
+mavlink_joystick_t joystick;
+
+unsigned len = 0;
+
+uint8_t system_id = 10;
+uint8_t component_id = 15;
+
+//Motordriver , RC MAP
+Motor motor_left(PA_9,PA_8,PB_15,1);
+Motor motor_right(PC_9,PC_6,PC_8,1);
+//pwm
+float joystick_percent_x = 0;
+float joystick_percent_y = 0;
+
+Map map_x (0, 1024, -0.3, 0.3);
+Map map_y (0, 1024, -0.3, 0.3);
+
+//Encoder
+IncrementalEncoder right_encoder(PD_2);
+IncrementalEncoder left_encoder(PC_4);
+
+//tof
+VL53L1X tof1 (PB_9, PB_8);
 
-void print_char(char c = '*')
+/////////////////////////////////////////////
+/////////////////write///////////////////////
+/////////////////////////////////////////////
+int write_data (char *data)
+{
+}
+void write_data(char* buf, unsigned int len)
+{
+    for(int i = 0; i<len; i++) {
+        xbee.putc(buf[i]);
+    }
+}
+
+void heartbeat_info()
+{
+    Thread::wait(2000);
+    heartbeat.type = 0;
+    heartbeat.mode = 1;
+
+    mavlink_msg_heartbeat_encode (system_id, component_id, &message, &heartbeat);
+    len = mavlink_msg_to_send_buffer((uint8_t*)buf, &message);
+
+    write_data(buf, len);
+}
+
+mavlink_motor_t motor;
+void Motor_message_send()
 {
-    printf("%c", c);
-    fflush(stdout);
+    Thread::wait(500);
+    motor.encoder_left = (uint16_t)left_encoder.readTotal();
+    motor.encoder_right = (uint16_t)right_encoder.readTotal();
+    
+    mavlink_msg_motor_encode (system_id, component_id, &message, &motor);
+    len = mavlink_msg_to_send_buffer((uint8_t*)buf, &message);
+    
+    write_data(buf, len);
+}
+mavlink_pwm_t pwm;
+void pwm_message_send()
+{
+    Thread::wait(500);
+    
+    pwm.motor_left_pwm = (uint16_t)joystick_percent_x;
+    pwm.motor_right_pwm = (uint16_t)joystick_percent_y;
+    
+    mavlink_msg_pwm_encode (system_id, component_id, &message, &pwm);
+    len=mavlink_msg_to_send_buffer((uint8_t*)buf, &message);
+    write_data(buf,len);    
+}
+
+void write_message()
+{
+    while(1) {
+        heartbeat_info();
+        Motor_message_send();
+        pwm_message_send();
+    }
 }
 
-Thread thread;
+/////////////////////////////////
+////////////read/////////////////
+/////////////////////////////////
+void object_detection()
+{
+    uint16_t distance = tof1.getDistance();
+    
+    if(distance < 400)
+    {
+        motor_left.speed (0);
+        motor_right.speed(0);
+    }
+}
+int state = NORMAL;
+float sens = 1;
+void speedGap()
+{
+    int gap = (right_encoder.readTotal()) - (left_encoder.readTotal());
+    
+    //left over
+    if (gap > 0 && sens < 1.5)
+    {
+        sens -= 0.01;
+    }
+    if (gap < 0 && sens > 0.5)
+    {
+        sens += 0.01;
+    }
+    
+}
+mavlink_joystick_t local_joystick;
 
-DigitalOut led1(LED1);
+void rc_move()
+{
+    local_joystick = joystick;
+    joystick_percent_x = map_x.Calculate(local_joystick.joystick_x);
+    joystick_percent_y = map_y.Calculate(local_joystick.joystick_y);
+    
+    if ( local_joystick.joystick_click == 0 ) {
+        motor_left.speed (0);
+        motor_right.speed (0);
+    }
+    //move forward/back
+    if( joystick_percent_y >= -0.1 && joystick_percent_y <= 0.1 && joystick_percent_x >= -0.1 && joystick_percent_x <=0.1) {
+        motor_left.speed (0);
+        motor_right.speed (0);
+        state = NORMAL;
+    }
 
-void print_thread()
+    else if ( joystick_percent_x > 0.1 && joystick_percent_y > 0.1) {
+        motor_left.speed (joystick_percent_y+joystick_percent_x);
+        motor_right.speed (joystick_percent_y*sens);
+    } 
+    else if (joystick_percent_x < -0.1 && joystick_percent_y >0.1) {
+        motor_left.speed (joystick_percent_y);
+        motor_right.speed ((joystick_percent_y +(-1*joystick_percent_x))*sens);
+    } 
+    else if (joystick_percent_x < -0.1 && joystick_percent_y < -0.1) {
+        motor_left.speed (joystick_percent_y );
+        motor_right.speed ((joystick_percent_y + joystick_percent_x)*sens);
+    } 
+    else if (joystick_percent_x > 0.1 && joystick_percent_y < - 0.1) {
+        motor_left.speed ((joystick_percent_y - joystick_percent_x));
+        motor_right.speed (joystick_percent_y*sens);
+    } 
+    //forward,backward
+    else if (joystick_percent_x < 0.1 && joystick_percent_x >-0.1 && joystick_percent_y>0.1) {
+        motor_left.speed (joystick_percent_y);
+        motor_right.speed (joystick_percent_y*sens);
+        state = BACK;
+    } 
+    else if (joystick_percent_x < 0.1 && joystick_percent_x >-0.1 && joystick_percent_y < -0.1) {
+        motor_left.speed (joystick_percent_y);
+        motor_right.speed (joystick_percent_y*sens);
+        state = FRONT;
+    } 
+    else if (joystick_percent_y < 0.1 && joystick_percent_y > -0.1 && joystick_percent_x < -0.1) {
+        motor_right.speed(-joystick_percent_x*sens);
+        state = LEFT;
+    } 
+    else if (joystick_percent_y < 0.1 && joystick_percent_y > -0.1 && joystick_percent_x > 0.1) {
+        motor_left.speed(joystick_percent_x);
+        state = RIGHT;
+    }
+        
+    if(state == FRONT)
+    {
+        speedGap();
+        object_detection();
+    }
+    else if(state == BACK || state == LEFT || state == RIGHT)
+    {
+        left_encoder.reset();
+        right_encoder.reset();
+    }
+    
+    
+}
+
+void read_data()
 {
-    while (true) {
-        wait(1);
-        print_char();
+    led = 1;
+    uint8_t msgReceived = 0;
+
+    while(1) {
+        if(xbee.readable()) {
+
+            uint8_t c = xbee.getc();
+
+            msgReceived = mavlink_parse_char(MAVLINK_COMM_1, c, &r_message, &r_status);
+
+            if(msgReceived > 0) {
+                led = 0;
+                if (r_message.sysid == 60|| r_message.sysid == 62) {
+                    switch (r_message.msgid) {
+                        case 3: { //JOYSTICK
+                            mavlink_msg_joystick_decode(&r_message, &joystick);
+                            rc_move();
+                            break;
+                        }
+                    }
+                } 
+            }
+        } else {
+            led = 1;
+        }
     }
 }
 
 int main()
 {
-    printf("\n\n*** RTOS basic example ***\n");
-
-    thread.start(print_thread);
-
-    while (true) {
-        led1 = !led1;
-        wait(0.5);
-    }
+    //tof i2c setting
+    tof1.begin();
+    tof1.setDistanceMode();
+    
+    thread.start(write_message);
+    read_data();
 }