Diff: kondo3.cpp

Revision:

4:99891561a38b

diff -r 1345f959c490 -r 99891561a38b kondo3.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/kondo3.cpp	Fri May 25 09:00:15 2018 +0000
@@ -0,0 +1,98 @@
+#include "mbed.h"
+#include "kondo3.h"
+#include "servo.h"
+#define KONDO3_COMMAND_POSITION_FREE 1
+#define KONDO3_COMMAND_POSITION_SET 2
+#define KONDO3_STEP_PER_DEGREE 8000/270
+RawSerial KONDO3_serial(PA_0,PA_1);
+unsigned char KONDO3_Buffer_Tx[3];
+unsigned char KONDO3_Buffer_Tx_Ptr = 0;
+unsigned char KONDO3_Buffer_Tx_Full = false;
+unsigned char KONDO3_Buffer_Rx[6];
+unsigned char KONDO3_Buffer_Rx_Ptr = 0;
+unsigned char KONDO3_Buffer_Rx_Full = false;
+signed short int KONDO3_Positions_Initial[] = {7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500};
+signed short int KONDO3_Positions_Current[KONDO3_MAX_TOTAL];
+signed short int KONDO3_Positions_Target[KONDO3_MAX_TOTAL];
+unsigned char KONDO3_SendOrder[]= {16,15,14,13,12,11};
+signed char KONDO3_Directions = 1;
+signed char KONDO3_ID = 0;
+signed char KONDO3_PtrCommands_Set;
+EventQueue KONDO3_queue;
+void KONDO3_Positions_Set(double* KONDO3_Degrees)
+{
+    unsigned char id;
+    for(KONDO3_ID=0;KONDO3_ID < sizeof(KONDO3_SendOrder);KONDO3_ID++)
+    {
+    id = KONDO3_SendOrder[KONDO3_ID];
+//            switch(KONDO1_PtrCommands_Set)
+//            {
+//                case KONDO1_COMMAND_POSITION_FREE:
+//                    KONDO1_Positions[id]=0;
+//                    break;
+//                KONDO1_COMMAND_POSITION_SET:
+    KONDO3_Positions_Target[id] = KONDO3_Degrees[id] * KONDO3_STEP_PER_DEGREE * KONDO3_Directions + KONDO3_Positions_Initial[id];
+//    wait(0.0008);
+    }  
+}
+void KONDO3_Interrupt_Tx() 
+{
+    while(KONDO3_serial.writeable())
+    {
+        KONDO3_serial.putc(KONDO3_Buffer_Tx[KONDO3_Buffer_Tx_Ptr]);
+        KONDO3_Buffer_Tx_Ptr++;
+        if(KONDO3_Buffer_Tx_Ptr==3)
+              KONDO3_Buffer_Tx_Ptr = 0;
+    }
+}
+void KONDO3_Interrupt_Rx()
+{
+    while(KONDO3_serial.readable())
+    {
+        KONDO3_Buffer_Rx[KONDO3_Buffer_Rx_Ptr] = KONDO3_serial.getc();
+        KONDO3_Buffer_Rx_Ptr++;
+        if(KONDO3_Buffer_Rx_Ptr==6)
+        {
+            KONDO3_Buffer_Rx_Full = true;
+            KONDO3_Buffer_Rx_Ptr=0;
+        }
+    }
+} 
+void KONDO3_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(USART1_IRQn);
+    KONDO3_Buffer_Tx[0] = id;
+    KONDO3_Buffer_Tx[1] = hi;
+    KONDO3_Buffer_Tx[2] = lo;
+//    wait(0.0008);
+    KONDO3_Buffer_Rx_Full = false;
+    NVIC_EnableIRQ(USART1_IRQn);
+}
+void KONDO3_task() 
+{
+    signed char id;
+    signed char i;
+    for(i=0 ; i < sizeof(KONDO3_SendOrder) ; i++)
+    {
+        id = KONDO3_SendOrder[i];
+        KONDO3_update(id, KONDO3_Positions_Target[id]);
+        wait_us(650);
+//        wait_us(825);
+    }
+}
+void KONDO3_init() 
+{
+    KONDO3_serial.format(8,Serial::Even,1);
+    KONDO3_serial.baud(115200);
+    KONDO3_Buffer_Rx_Ptr = 0;
+    KONDO3_Buffer_Rx_Full = false;
+    KONDO3_serial.attach(&KONDO3_Interrupt_Rx, Serial::RxIrq);
+    KONDO3_serial.attach(&KONDO3_Interrupt_Tx, Serial::TxIrq);
+    NVIC_EnableIRQ(USART1_IRQn);
+    KONDO3_queue.call_every(10, KONDO3_task); //1000
+    KONDO3_queue.dispatch();
+}
\ No newline at end of file