Diff: servo.cpp

Revision:

3:1345f959c490

Parent:

2:7d574b1ab3cd

Child:

4:45bd9ba6c0da

--- a/servo.cpp	Wed May 02 03:33:15 2018 +0000
+++ b/servo.cpp	Wed May 02 07:49:13 2018 +0000
@@ -1,9 +1,6 @@
 #include "mbed.h"
 #include "rtos.h"
-DigitalOut l1(LED1);
-DigitalOut l2(LED2);
-DigitalOut l3(LED3);
-DigitalOut l4(LED4);
+DigitalOut led3(LED3);
 RawSerial motor(PC_4,PC_5);
 unsigned char tx_buffer[3];
 unsigned char tx_buffer_ptr = 0;
@@ -18,6 +15,7 @@
 void Rx_interrupt();
 //EventQueue queue;
 //Thread t;
+
 void motor_update(unsigned char Id, unsigned short int Position)
 {
     unsigned char id,lo,hi;
@@ -32,6 +30,7 @@
 }
 void SERVO_task() 
 {
+    led3 = !led3;
     for(MotorID=0;MotorID<=1;MotorID++)
     {
         motor_update(MotorID,motorcp[MotorID]);
@@ -40,7 +39,6 @@
 }
 void SERVO_init() 
 {
-    l1 = !l1;
     motor.format(8,Serial::Even,1);
     motor.baud(115200);
     rx_buffer_ptr = 0;
@@ -48,8 +46,9 @@
     motor.attach(&Rx_interrupt, Serial::RxIrq);
     motor.attach(&Tx_interrupt, Serial::TxIrq);
     NVIC_EnableIRQ(USART3_IRQn);
-//    t.start(callback(&queue, &EventQueue::dispatch_forever));
+    //t.start(callback(&queue, &EventQueue::dispatch_forever));
     //queue.call_every(1000, SERVO_init);
+/*
     while(1)
     {
         motorcp[0]=7500;
@@ -59,6 +58,7 @@
         motorcp[1]=8500;
         Thread::wait(1000);
     }
+*/
 }
 
 void Tx_interrupt()