Diff: kondo2.cpp

Revision:

4:45bd9ba6c0da

diff -r 1345f959c490 -r 45bd9ba6c0da kondo2.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/kondo2.cpp	Thu May 24 08:29:52 2018 +0000
@@ -0,0 +1,96 @@
+#include "mbed.h"
+#include "kondo2.h"
+#include "servo.h"
+#define KONDO2_COMMAND_POSITION_FREE 1
+#define KONDO2_COMMAND_POSITION_SET 2
+#define KONDO2_STEP_PER_DEGREE 8000/270
+RawSerial KONDO2_serial(PA_2,PA_3);
+unsigned char KONDO2_Buffer_Tx[3];
+unsigned char KONDO2_Buffer_Tx_Ptr = 0;
+unsigned char KONDO2_Buffer_Tx_Full = false;
+unsigned char KONDO2_Buffer_Rx[6];
+unsigned char KONDO2_Buffer_Rx_Ptr = 0;
+unsigned char KONDO2_Buffer_Rx_Full = false;
+signed short int KONDO2_Positions_Initial[] = {7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500};
+signed short int KONDO2_Positions_Current[KONDO2_MAX_TOTAL];
+signed short int KONDO2_Positions_Target[KONDO2_MAX_TOTAL];
+unsigned char KONDO2_SendOrder[]= {7,9,10};
+signed char KONDO2_Directions = 1;
+signed char KONDO2_PtrCommands_Set;
+signed char KONDO2_ID = 0;
+EventQueue KONDO2_queue;
+void KONDO2_Positions_Set(double* KONDO2_Degrees)
+{
+        unsigned char id;
+        for(KONDO2_ID=0;KONDO2_ID < sizeof(KONDO2_SendOrder);KONDO2_ID++)
+        {
+        id = KONDO2_SendOrder[KONDO2_ID];
+        KONDO2_Positions_Target[id] = KONDO2_Degrees[id] * KONDO2_STEP_PER_DEGREE * KONDO2_Directions + KONDO2_Positions_Initial[id];
+//        wait(0.0008);
+        }
+}
+void KONDO2_Interrupt_Tx() 
+{
+    while(KONDO2_serial.writeable())
+    {
+        KONDO2_serial.putc(KONDO2_Buffer_Tx[KONDO2_Buffer_Tx_Ptr]);
+//        KONDO2_Buffer_Rx[KONDO2_Buffer_Rx_Ptr] = KONDO2_serial.getc();
+        KONDO2_Buffer_Tx_Ptr++;
+ //       KONDO2_Buffer_Rx_Ptr++;
+        if(KONDO2_Buffer_Tx_Ptr==3 && KONDO2_Buffer_Rx_Ptr==3)
+              KONDO2_Buffer_Tx_Ptr = 0;
+//              KONDO2_Buffer_Rx_Ptr = 0;
+    }
+}
+void KONDO2_Interrupt_Rx()
+{
+    while(KONDO2_serial.readable())
+    {
+ //       KONDO2_Buffer_Rx_Ptr = 3;
+        KONDO2_Buffer_Rx[KONDO2_Buffer_Rx_Ptr] = KONDO2_serial.getc();
+        KONDO2_Buffer_Rx_Ptr++;
+        if(KONDO2_Buffer_Rx_Ptr==6)
+        {
+            KONDO2_Buffer_Rx_Full = true;
+            KONDO2_Buffer_Rx_Ptr=0;
+        }
+    }
+} 
+void KONDO2_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(USART2_IRQn);
+    KONDO2_Buffer_Tx[0] = id;
+    KONDO2_Buffer_Tx[1] = hi;
+    KONDO2_Buffer_Tx[2] = lo;
+//    wait(0.0008);
+    KONDO2_Buffer_Rx_Full = false;
+    NVIC_EnableIRQ(USART2_IRQn);
+}
+void KONDO2_task() 
+{
+    signed char id;
+    signed char i;
+    for(i=0 ; i < sizeof(KONDO2_SendOrder) ; i++)
+    {
+        id = KONDO2_SendOrder[i];
+        KONDO2_update(id, KONDO2_Positions_Target[id]);
+//        wait_us(825);
+        wait_us(650);
+    }
+}
+void KONDO2_init() 
+{
+    KONDO2_serial.format(8,Serial::Even,1);
+    KONDO2_serial.baud(115200);
+    KONDO2_Buffer_Rx_Ptr = 0;
+    KONDO2_Buffer_Rx_Full = false;
+    KONDO2_serial.attach(&KONDO2_Interrupt_Rx, Serial::RxIrq);
+    KONDO2_serial.attach(&KONDO2_Interrupt_Tx, Serial::TxIrq);
+    NVIC_EnableIRQ(USART2_IRQn);    
+    KONDO2_queue.call_every(10, KONDO2_task);
+    KONDO2_queue.dispatch();
+}