Lin ShengKun / Mbed OS NYP_FYP

FYP

Dependencies:   LSM6DSL

Fork of humanoid by Junjie Wang

kondo3.cpp@4:45bd9ba6c0da, 2018-05-24 (annotated)

Committer:
ha731548874
Date:
Thu May 24 08:29:52 2018 +0000
Revision:
4:45bd9ba6c0da
NYP_humaniod_FYP

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ha731548874 4:45bd9ba6c0da 1 #include "mbed.h"
ha731548874 4:45bd9ba6c0da 2 #include "kondo3.h"
ha731548874 4:45bd9ba6c0da 3 #include "servo.h"
ha731548874 4:45bd9ba6c0da 4 #define KONDO3_COMMAND_POSITION_FREE 1
ha731548874 4:45bd9ba6c0da 5 #define KONDO3_COMMAND_POSITION_SET 2
ha731548874 4:45bd9ba6c0da 6 #define KONDO3_STEP_PER_DEGREE 8000/270
ha731548874 4:45bd9ba6c0da 7 RawSerial KONDO3_serial(PA_0,PA_1);
ha731548874 4:45bd9ba6c0da 8 unsigned char KONDO3_Buffer_Tx[3];
ha731548874 4:45bd9ba6c0da 9 unsigned char KONDO3_Buffer_Tx_Ptr = 0;
ha731548874 4:45bd9ba6c0da 10 unsigned char KONDO3_Buffer_Tx_Full = false;
ha731548874 4:45bd9ba6c0da 11 unsigned char KONDO3_Buffer_Rx[6];
ha731548874 4:45bd9ba6c0da 12 unsigned char KONDO3_Buffer_Rx_Ptr = 0;
ha731548874 4:45bd9ba6c0da 13 unsigned char KONDO3_Buffer_Rx_Full = false;
ha731548874 4:45bd9ba6c0da 14 signed short int KONDO3_Positions_Initial[] = {7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500};
ha731548874 4:45bd9ba6c0da 15 signed short int KONDO3_Positions_Current[KONDO3_MAX_TOTAL];
ha731548874 4:45bd9ba6c0da 16 signed short int KONDO3_Positions_Target[KONDO3_MAX_TOTAL];
ha731548874 4:45bd9ba6c0da 17 unsigned char KONDO3_SendOrder[]= {16,15,14,13,12,11};
ha731548874 4:45bd9ba6c0da 18 signed char KONDO3_Directions = 1;
ha731548874 4:45bd9ba6c0da 19 signed char KONDO3_ID = 0;
ha731548874 4:45bd9ba6c0da 20 signed char KONDO3_PtrCommands_Set;
ha731548874 4:45bd9ba6c0da 21 EventQueue KONDO3_queue;
ha731548874 4:45bd9ba6c0da 22 void KONDO3_Positions_Set(double* KONDO3_Degrees)
ha731548874 4:45bd9ba6c0da 23 {
ha731548874 4:45bd9ba6c0da 24 unsigned char id;
ha731548874 4:45bd9ba6c0da 25 for(KONDO3_ID=0;KONDO3_ID < sizeof(KONDO3_SendOrder);KONDO3_ID++)
ha731548874 4:45bd9ba6c0da 26 {
ha731548874 4:45bd9ba6c0da 27 id = KONDO3_SendOrder[KONDO3_ID];
ha731548874 4:45bd9ba6c0da 28 // switch(KONDO1_PtrCommands_Set)
ha731548874 4:45bd9ba6c0da 29 // {
ha731548874 4:45bd9ba6c0da 30 // case KONDO1_COMMAND_POSITION_FREE:
ha731548874 4:45bd9ba6c0da 31 // KONDO1_Positions[id]=0;
ha731548874 4:45bd9ba6c0da 32 // break;
ha731548874 4:45bd9ba6c0da 33 // KONDO1_COMMAND_POSITION_SET:
ha731548874 4:45bd9ba6c0da 34 KONDO3_Positions_Target[id] = KONDO3_Degrees[id] * KONDO3_STEP_PER_DEGREE * KONDO3_Directions + KONDO3_Positions_Initial[id];
ha731548874 4:45bd9ba6c0da 35 // wait(0.0008);
ha731548874 4:45bd9ba6c0da 36 }
ha731548874 4:45bd9ba6c0da 37 }
ha731548874 4:45bd9ba6c0da 38 void KONDO3_Interrupt_Tx()
ha731548874 4:45bd9ba6c0da 39 {
ha731548874 4:45bd9ba6c0da 40 while(KONDO3_serial.writeable())
ha731548874 4:45bd9ba6c0da 41 {
ha731548874 4:45bd9ba6c0da 42 KONDO3_serial.putc(KONDO3_Buffer_Tx[KONDO3_Buffer_Tx_Ptr]);
ha731548874 4:45bd9ba6c0da 43 KONDO3_Buffer_Tx_Ptr++;
ha731548874 4:45bd9ba6c0da 44 if(KONDO3_Buffer_Tx_Ptr==3)
ha731548874 4:45bd9ba6c0da 45 KONDO3_Buffer_Tx_Ptr = 0;
ha731548874 4:45bd9ba6c0da 46 }
ha731548874 4:45bd9ba6c0da 47 }
ha731548874 4:45bd9ba6c0da 48 void KONDO3_Interrupt_Rx()
ha731548874 4:45bd9ba6c0da 49 {
ha731548874 4:45bd9ba6c0da 50 while(KONDO3_serial.readable())
ha731548874 4:45bd9ba6c0da 51 {
ha731548874 4:45bd9ba6c0da 52 KONDO3_Buffer_Rx[KONDO3_Buffer_Rx_Ptr] = KONDO3_serial.getc();
ha731548874 4:45bd9ba6c0da 53 KONDO3_Buffer_Rx_Ptr++;
ha731548874 4:45bd9ba6c0da 54 if(KONDO3_Buffer_Rx_Ptr==6)
ha731548874 4:45bd9ba6c0da 55 {
ha731548874 4:45bd9ba6c0da 56 KONDO3_Buffer_Rx_Full = true;
ha731548874 4:45bd9ba6c0da 57 KONDO3_Buffer_Rx_Ptr=0;
ha731548874 4:45bd9ba6c0da 58 }
ha731548874 4:45bd9ba6c0da 59 }
ha731548874 4:45bd9ba6c0da 60 }
ha731548874 4:45bd9ba6c0da 61 void KONDO3_update(unsigned char Id, unsigned short int Position)
ha731548874 4:45bd9ba6c0da 62 {
ha731548874 4:45bd9ba6c0da 63 unsigned char id,lo,hi;
ha731548874 4:45bd9ba6c0da 64 id=0x80|Id;
ha731548874 4:45bd9ba6c0da 65 hi=(Position>>7)&0x007F;
ha731548874 4:45bd9ba6c0da 66 lo=Position&0x007F;
ha731548874 4:45bd9ba6c0da 67 NVIC_DisableIRQ(USART1_IRQn);
ha731548874 4:45bd9ba6c0da 68 KONDO3_Buffer_Tx[0] = id;
ha731548874 4:45bd9ba6c0da 69 KONDO3_Buffer_Tx[1] = hi;
ha731548874 4:45bd9ba6c0da 70 KONDO3_Buffer_Tx[2] = lo;
ha731548874 4:45bd9ba6c0da 71 // wait(0.0008);
ha731548874 4:45bd9ba6c0da 72 KONDO3_Buffer_Rx_Full = false;
ha731548874 4:45bd9ba6c0da 73 NVIC_EnableIRQ(USART1_IRQn);
ha731548874 4:45bd9ba6c0da 74 }
ha731548874 4:45bd9ba6c0da 75 void KONDO3_task()
ha731548874 4:45bd9ba6c0da 76 {
ha731548874 4:45bd9ba6c0da 77 signed char id;
ha731548874 4:45bd9ba6c0da 78 signed char i;
ha731548874 4:45bd9ba6c0da 79 for(i=0 ; i < sizeof(KONDO3_SendOrder) ; i++)
ha731548874 4:45bd9ba6c0da 80 {
ha731548874 4:45bd9ba6c0da 81 id = KONDO3_SendOrder[i];
ha731548874 4:45bd9ba6c0da 82 KONDO3_update(id, KONDO3_Positions_Target[id]);
ha731548874 4:45bd9ba6c0da 83 wait_us(650);
ha731548874 4:45bd9ba6c0da 84 // wait_us(825);
ha731548874 4:45bd9ba6c0da 85 }
ha731548874 4:45bd9ba6c0da 86 }
ha731548874 4:45bd9ba6c0da 87 void KONDO3_init()
ha731548874 4:45bd9ba6c0da 88 {
ha731548874 4:45bd9ba6c0da 89 KONDO3_serial.format(8,Serial::Even,1);
ha731548874 4:45bd9ba6c0da 90 KONDO3_serial.baud(115200);
ha731548874 4:45bd9ba6c0da 91 KONDO3_Buffer_Rx_Ptr = 0;
ha731548874 4:45bd9ba6c0da 92 KONDO3_Buffer_Rx_Full = false;
ha731548874 4:45bd9ba6c0da 93 KONDO3_serial.attach(&KONDO3_Interrupt_Rx, Serial::RxIrq);
ha731548874 4:45bd9ba6c0da 94 KONDO3_serial.attach(&KONDO3_Interrupt_Tx, Serial::TxIrq);
ha731548874 4:45bd9ba6c0da 95 NVIC_EnableIRQ(USART1_IRQn);
ha731548874 4:45bd9ba6c0da 96 KONDO3_queue.call_every(10, KONDO3_task); //1000
ha731548874 4:45bd9ba6c0da 97 KONDO3_queue.dispatch();
ha731548874 4:45bd9ba6c0da 98 }