Diff: servo.cpp

Revision:

2:7d574b1ab3cd

Parent:

1:cde16b5e604d

Child:

3:1345f959c490

--- a/servo.cpp	Mon Apr 30 11:23:06 2018 +0000
+++ b/servo.cpp	Wed May 02 03:33:15 2018 +0000
@@ -1,6 +1,5 @@
 #include "mbed.h"
 #include "rtos.h"
-
 DigitalOut l1(LED1);
 DigitalOut l2(LED2);
 DigitalOut l3(LED3);
@@ -17,7 +16,8 @@
 int MotorID;
 void Tx_interrupt();
 void Rx_interrupt();
-
+//EventQueue queue;
+//Thread t;
 void motor_update(unsigned char Id, unsigned short int Position)
 {
     unsigned char id,lo,hi;
@@ -30,7 +30,7 @@
     tx_buffer[2] = lo;
     NVIC_EnableIRQ(USART3_IRQn);
 }
-void SERVO_init() 
+void SERVO_task() 
 {
     for(MotorID=0;MotorID<=1;MotorID++)
     {
@@ -38,8 +38,9 @@
         wait_us(900);
     }
 }
-void SERVO_task() 
+void SERVO_init() 
 {
+    l1 = !l1;
     motor.format(8,Serial::Even,1);
     motor.baud(115200);
     rx_buffer_ptr = 0;
@@ -47,6 +48,8 @@
     motor.attach(&Rx_interrupt, Serial::RxIrq);
     motor.attach(&Tx_interrupt, Serial::TxIrq);
     NVIC_EnableIRQ(USART3_IRQn);
+//    t.start(callback(&queue, &EventQueue::dispatch_forever));
+    //queue.call_every(1000, SERVO_init);
     while(1)
     {
         motorcp[0]=7500;