kondo1.cpp

00001 #include "mbed.h"
00002 #include "kondo1.h"
00003 #include "servo.h"
00004 #define KONDO1_COMMAND_POSITION_FREE 1
00005 #define KONDO1_COMMAND_POSITION_SET 2
00006 #define KONDO1_STEP_PER_DEGREE 8000/270
00007 RawSerial KONDO1_serial(PC_4,PC_5);
00008 unsigned char KONDO1_Buffer_Tx[3];
00009 unsigned char KONDO1_Buffer_Tx_Ptr = 0;
00010 unsigned char KONDO1_Buffer_Tx_Full = false;
00011 unsigned char KONDO1_Buffer_Rx[6];
00012 unsigned char KONDO1_Buffer_Rx_Ptr = 3;
00013 unsigned char KONDO1_Buffer_Rx_Full = false;
00014 signed short int KONDO1_Positions_Initial[] = {7500,7500,7500,7500,7500,7500,7500};
00015 signed short int KONDO1_Positions_Target[KONDO1_MAX_TOTAL];
00016 //signed short int KONDO1_Positions_Current[KONDO1_MAX_TOTAL]= {7500,7500,7500,7500,7500};
00017 unsigned char KONDO1_SendOrder[]= {1,2,3,4,5,6};
00018 signed char KONDO1_Directions = 1;
00019 signed char KONDO1_ID = 0;
00020 signed char KONDO1_PtrCommands_Set;
00021 EventQueue KONDO1_queue;
00022 /*signed short int KONDO1_Position_Convert(char hi, char lo)
00023 {
00024     signed short int i = hi;
00025     i = i<<7;
00026     i = i|lo;
00027     return i;
00028 }
00029 void KONDO1_Position_Current_Set()
00030 {
00031     unsigned char id;
00032     id = KONDO1_Buffer_Rx[0]&0x1F;
00033     KONDO1_Positions_Current[id] = KONDO1_Position_Convert(KONDO1_Buffer_Rx[4], KONDO1_Buffer_Rx[5]);
00034 }*/
00035 void KONDO1_Positions_Set(double* KONDO1_Degrees)
00036 {
00037     unsigned char id;
00038     for(KONDO1_ID=0;KONDO1_ID < sizeof(KONDO1_SendOrder);KONDO1_ID++)
00039     {
00040         id = KONDO1_SendOrder[KONDO1_ID];
00041 //            switch(KONDO1_PtrCommands_Set)
00042 //            {
00043 //                case KONDO1_COMMAND_POSITION_FREE:
00044 //                    KONDO1_Positions[id]=0;
00045 //                    break;
00046 //                case KONDO1_COMMAND_POSITION_SET:
00047         KONDO1_Positions_Target[id] = KONDO1_Degrees[id] * KONDO1_STEP_PER_DEGREE * KONDO1_Directions + KONDO1_Positions_Initial[id];
00048 //                    break;
00049 //                  case KONDO1_COMMAND_DEGREE_SET:
00050 //                    KONDO1_degrees_Target[id] = (KONDO1_Positions_Target[id] - KONDO1_Positions_Initial[id])/ (KONDO1_STEP_PER_DEGREE * KONDO1_Directions);
00051 //                    break;
00052 //wait(0.000625);
00053     }
00054 }
00055 void KONDO1_Interrupt_Tx()
00056 {
00057     while(KONDO1_serial.writeable())
00058     {
00059         KONDO1_serial.putc(KONDO1_Buffer_Tx[KONDO1_Buffer_Tx_Ptr]);
00060         KONDO1_Buffer_Tx_Ptr++;
00061         if(KONDO1_Buffer_Tx_Ptr==3)
00062               KONDO1_Buffer_Tx_Ptr = 0;
00063     }
00064 }
00065 void KONDO1_Interrupt_Rx()
00066 {
00067     while(KONDO1_serial.readable())
00068     {
00069         KONDO1_Buffer_Rx[KONDO1_Buffer_Rx_Ptr] = KONDO1_serial.getc();
00070         KONDO1_Buffer_Rx_Ptr++;
00071         if(KONDO1_Buffer_Rx_Ptr==6)
00072         {
00073             KONDO1_Buffer_Rx_Full = true;
00074             KONDO1_Buffer_Rx_Ptr=0;
00075 //            KONDO1_Position_Current_Set();
00076         }
00077     }
00078 } 
00079 void KONDO1_update(unsigned char Id, unsigned short int Position)
00080 {
00081     unsigned char id,lo,hi;
00082     id=0x80|Id;
00083     hi=(Position>>7)&0x007F;
00084     lo=Position&0x007F;
00085     NVIC_DisableIRQ(USART3_IRQn);
00086     KONDO1_Buffer_Tx[0] = id;// KONDO1_Buffer_Rx[0] = KONDO1_serial.getc();
00087     KONDO1_Buffer_Tx[1] = hi;// KONDO1_Buffer_Rx[1] = KONDO1_serial.getc();
00088     KONDO1_Buffer_Tx[2] = lo;// KONDO1_Buffer_Rx[2] = KONDO1_serial.getc();
00089 //    wait(0.0007);      
00090     KONDO1_Buffer_Rx_Full = false;
00091     NVIC_EnableIRQ(USART3_IRQn);
00092 }
00093 void KONDO1_task() 
00094 {
00095     signed char id;
00096     signed char i;
00097     for(i=0 ; i < sizeof(KONDO1_SendOrder) ; i++)
00098     {
00099         id = KONDO1_SendOrder[i];
00100 //        if(id==0)KONDO1_update(id, 0);else
00101         KONDO1_update(id, KONDO1_Positions_Target[id]);
00102         wait_us(650);    //650   1000
00103 //        printf("%d=%d\r\n",id,KONDO1_Positions_Current[id]);
00104 //        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]);
00105     }
00106 }
00107 void KONDO1_init()
00108 {
00109     KONDO1_serial.format(8,Serial::Even,1);
00110     KONDO1_serial.baud(115200);
00111     KONDO1_Buffer_Rx_Ptr = 0;
00112     KONDO1_Buffer_Rx_Full = false;
00113     KONDO1_serial.attach(&KONDO1_Interrupt_Rx, Serial::RxIrq);
00114     KONDO1_serial.attach(&KONDO1_Interrupt_Tx, Serial::TxIrq);
00115     NVIC_EnableIRQ(USART3_IRQn);
00116     KONDO1_queue.call_every(10, KONDO1_task);   //1000    0.1  1
00117     KONDO1_queue.dispatch();
00118 }