Diff: main.cpp

Revision:

8:075a9610e2cd

Parent:

7:bd0aa7f21f53

diff -r bd0aa7f21f53 -r 075a9610e2cd main.cpp
--- a/main.cpp	Wed Feb 15 13:58:42 2017 -0600
+++ b/main.cpp	Wed May 02 03:30:28 2018 +0000
@@ -1,27 +1,99 @@
 #include "mbed.h"
 #include "rtos.h"
-
-Mutex stdio_mutex;
-
-void notify(const char* name, int state) {
-    stdio_mutex.lock();
-    printf("%s: %d\n\r", name, state);
-    stdio_mutex.unlock();
+DigitalOut l1(LED1);
+DigitalOut l2(LED2);
+DigitalOut l3(LED3);
+DigitalOut l4(LED4);
+Thread servo;
+//Mutex stdio_mutex;
+RawSerial motor(PC_4,PC_5);
+unsigned char tx_buffer[3];
+unsigned char tx_buffer_ptr = 0;
+unsigned char tx_buffer_full = false;
+unsigned char rx_buffer[6];
+unsigned char rx_buffer_ptr = 0;
+unsigned char rx_buffer_full = false;
+signed short int motorcp[] = {8500,8500};
+unsigned short int Position;
+int MotorID;
+void Tx_interrupt();
+void Rx_interrupt();
+void motor_update(unsigned char Id, unsigned short int Position)
+{
+    unsigned char id,lo,hi;
+    id=0x80|Id;
+    hi=(Position>>7)&0x007F;
+    lo=Position&0x007F;
+    NVIC_DisableIRQ(USART3_IRQn);
+    tx_buffer[0] = id;
+    tx_buffer[1] = hi;
+    tx_buffer[2] = lo;
+//    wait(0.0008);
+    NVIC_EnableIRQ(USART3_IRQn);
 }
-
-void test_thread(void const *args) {
-    while (true) {
-        notify((const char*)args, 0); Thread::wait(1000);
-        notify((const char*)args, 1); Thread::wait(1000);
+void tx_thread() 
+{
+//    stdio_mutex.lock();
+    for(MotorID=0;MotorID<=1;MotorID++)
+    {
+        motor_update(MotorID,motorcp[MotorID]);
+        wait_us(900);
+    }
+//    stdio_mutex.unlock();
+}
+void SERVO_thread() 
+{
+    motor.format(8,Serial::Even,1);
+    motor.baud(115200);
+    rx_buffer_ptr = 0;
+    rx_buffer_full = false;
+    motor.attach(&Rx_interrupt, Serial::RxIrq);
+    motor.attach(&Tx_interrupt, Serial::TxIrq);
+    NVIC_EnableIRQ(USART3_IRQn);
+    RtosTimer motorposition(tx_thread);
+    motorposition.start(10);
+    while(1)
+    {
+        motorcp[0]=7500;
+        motorcp[1]=7500;
+        Thread::wait(1000);
+        motorcp[0]=8500;
+        motorcp[1]=8500;
+        Thread::wait(1000);
     }
 }
-
-int main() {
-    Thread t2;
-    Thread t3;
-
-    t2.start(callback(test_thread, (void *)"Th 2"));
-    t3.start(callback(test_thread, (void *)"Th 3"));
-
-    test_thread((void *)"Th 1");
+int main()
+{   
+    servo.start(SERVO_thread);     
+    while (1) 
+    { 
+        l1=!l1;
+        Thread::wait(1000);
+//       printf("Tx : %d %x %x\r\n",tx_buffer[0],tx_buffer[1],tx_buffer[2]);
+//        printf("RX : %d %x %x %d %x %x \r\n",rx_buffer[0], rx_buffer[1],rx_buffer[2],rx_buffer[3],rx_buffer[4],rx_buffer[5]);  
+    }
 }
+void Tx_interrupt() 
+{
+    while(motor.writeable())
+    {    
+        motor.putc(tx_buffer[tx_buffer_ptr]);
+        tx_buffer_ptr++;
+        if(tx_buffer_ptr==3)
+              tx_buffer_ptr = 0;
+    }
+    return;
+}
+void Rx_interrupt()
+{
+    while(motor.readable())
+    {
+        rx_buffer[rx_buffer_ptr] = motor.getc();
+        rx_buffer_ptr++;
+        if(rx_buffer_ptr==6)
+        {
+            rx_buffer_full = true;
+            rx_buffer_ptr=0;
+        }
+    }
+}