Diff: kondo1.cpp

Revision:

4:45bd9ba6c0da

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/kondo1.cpp	Thu May 24 08:29:52 2018 +0000
@@ -0,0 +1,118 @@
+#include "mbed.h"
+#include "kondo1.h"
+#include "servo.h"
+#define KONDO1_COMMAND_POSITION_FREE 1
+#define KONDO1_COMMAND_POSITION_SET 2
+#define KONDO1_STEP_PER_DEGREE 8000/270
+RawSerial KONDO1_serial(PC_4,PC_5);
+unsigned char KONDO1_Buffer_Tx[3];
+unsigned char KONDO1_Buffer_Tx_Ptr = 0;
+unsigned char KONDO1_Buffer_Tx_Full = false;
+unsigned char KONDO1_Buffer_Rx[6];
+unsigned char KONDO1_Buffer_Rx_Ptr = 3;
+unsigned char KONDO1_Buffer_Rx_Full = false;
+signed short int KONDO1_Positions_Initial[] = {7500,7500,7500,7500,7500,7500,7500};
+signed short int KONDO1_Positions_Target[KONDO1_MAX_TOTAL];
+//signed short int KONDO1_Positions_Current[KONDO1_MAX_TOTAL]= {7500,7500,7500,7500,7500};
+unsigned char KONDO1_SendOrder[]= {1,2,3,4,5,6};
+signed char KONDO1_Directions = 1;
+signed char KONDO1_ID = 0;
+signed char KONDO1_PtrCommands_Set;
+EventQueue KONDO1_queue;
+/*signed short int KONDO1_Position_Convert(char hi, char lo)
+{
+    signed short int i = hi;
+    i = i<<7;
+    i = i|lo;
+    return i;
+}
+void KONDO1_Position_Current_Set()
+{
+    unsigned char id;
+    id = KONDO1_Buffer_Rx[0]&0x1F;
+    KONDO1_Positions_Current[id] = KONDO1_Position_Convert(KONDO1_Buffer_Rx[4], KONDO1_Buffer_Rx[5]);
+}*/
+void KONDO1_Positions_Set(double* KONDO1_Degrees)
+{
+    unsigned char id;
+    for(KONDO1_ID=0;KONDO1_ID < sizeof(KONDO1_SendOrder);KONDO1_ID++)
+    {
+        id = KONDO1_SendOrder[KONDO1_ID];
+//            switch(KONDO1_PtrCommands_Set)
+//            {
+//                case KONDO1_COMMAND_POSITION_FREE:
+//                    KONDO1_Positions[id]=0;
+//                    break;
+//                case KONDO1_COMMAND_POSITION_SET:
+        KONDO1_Positions_Target[id] = KONDO1_Degrees[id] * KONDO1_STEP_PER_DEGREE * KONDO1_Directions + KONDO1_Positions_Initial[id];
+//                    break;
+//                  case KONDO1_COMMAND_DEGREE_SET:
+//                    KONDO1_degrees_Target[id] = (KONDO1_Positions_Target[id] - KONDO1_Positions_Initial[id])/ (KONDO1_STEP_PER_DEGREE * KONDO1_Directions);
+//                    break;
+//wait(0.000625);
+    }
+}
+void KONDO1_Interrupt_Tx()
+{
+    while(KONDO1_serial.writeable())
+    {
+        KONDO1_serial.putc(KONDO1_Buffer_Tx[KONDO1_Buffer_Tx_Ptr]);
+        KONDO1_Buffer_Tx_Ptr++;
+        if(KONDO1_Buffer_Tx_Ptr==3)
+              KONDO1_Buffer_Tx_Ptr = 0;
+    }
+}
+void KONDO1_Interrupt_Rx()
+{
+    while(KONDO1_serial.readable())
+    {
+        KONDO1_Buffer_Rx[KONDO1_Buffer_Rx_Ptr] = KONDO1_serial.getc();
+        KONDO1_Buffer_Rx_Ptr++;
+        if(KONDO1_Buffer_Rx_Ptr==6)
+        {
+            KONDO1_Buffer_Rx_Full = true;
+            KONDO1_Buffer_Rx_Ptr=0;
+//            KONDO1_Position_Current_Set();
+        }
+    }
+} 
+void KONDO1_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);
+    KONDO1_Buffer_Tx[0] = id;// KONDO1_Buffer_Rx[0] = KONDO1_serial.getc();
+    KONDO1_Buffer_Tx[1] = hi;// KONDO1_Buffer_Rx[1] = KONDO1_serial.getc();
+    KONDO1_Buffer_Tx[2] = lo;// KONDO1_Buffer_Rx[2] = KONDO1_serial.getc();
+//    wait(0.0007);      
+    KONDO1_Buffer_Rx_Full = false;
+    NVIC_EnableIRQ(USART3_IRQn);
+}
+void KONDO1_task() 
+{
+    signed char id;
+    signed char i;
+    for(i=0 ; i < sizeof(KONDO1_SendOrder) ; i++)
+    {
+        id = KONDO1_SendOrder[i];
+//        if(id==0)KONDO1_update(id, 0);else
+        KONDO1_update(id, KONDO1_Positions_Target[id]);
+        wait_us(650);    //650   1000
+//        printf("%d=%d\r\n",id,KONDO1_Positions_Current[id]);
+//        printf("%x,%x,%x,%x,%x,%x\r\n",KONDO1_Buffer_Rx[0],KONDO1_Buffer_Rx[1],KONDO1_Buffer_Rx[2],KONDO1_Buffer_Rx[3],KONDO1_Buffer_Rx[4],KONDO1_Buffer_Rx[5]);
+    }
+}
+void KONDO1_init()
+{
+    KONDO1_serial.format(8,Serial::Even,1);
+    KONDO1_serial.baud(115200);
+    KONDO1_Buffer_Rx_Ptr = 0;
+    KONDO1_Buffer_Rx_Full = false;
+    KONDO1_serial.attach(&KONDO1_Interrupt_Rx, Serial::RxIrq);
+    KONDO1_serial.attach(&KONDO1_Interrupt_Tx, Serial::TxIrq);
+    NVIC_EnableIRQ(USART3_IRQn);
+    KONDO1_queue.call_every(10, KONDO1_task);   //1000    0.1  1
+    KONDO1_queue.dispatch();
+}
\ No newline at end of file