Lin ShengKun / Mbed OS NYP_FYP

FYP

Dependencies:   LSM6DSL

Fork of humanoid by Junjie Wang

kondo2.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 "kondo2.h"
ha731548874 4:45bd9ba6c0da 3 #include "servo.h"
ha731548874 4:45bd9ba6c0da 4 #define KONDO2_COMMAND_POSITION_FREE 1
ha731548874 4:45bd9ba6c0da 5 #define KONDO2_COMMAND_POSITION_SET 2
ha731548874 4:45bd9ba6c0da 6 #define KONDO2_STEP_PER_DEGREE 8000/270
ha731548874 4:45bd9ba6c0da 7 RawSerial KONDO2_serial(PA_2,PA_3);
ha731548874 4:45bd9ba6c0da 8 unsigned char KONDO2_Buffer_Tx[3];
ha731548874 4:45bd9ba6c0da 9 unsigned char KONDO2_Buffer_Tx_Ptr = 0;
ha731548874 4:45bd9ba6c0da 10 unsigned char KONDO2_Buffer_Tx_Full = false;
ha731548874 4:45bd9ba6c0da 11 unsigned char KONDO2_Buffer_Rx[6];
ha731548874 4:45bd9ba6c0da 12 unsigned char KONDO2_Buffer_Rx_Ptr = 0;
ha731548874 4:45bd9ba6c0da 13 unsigned char KONDO2_Buffer_Rx_Full = false;
ha731548874 4:45bd9ba6c0da 14 signed short int KONDO2_Positions_Initial[] = {7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500};
ha731548874 4:45bd9ba6c0da 15 signed short int KONDO2_Positions_Current[KONDO2_MAX_TOTAL];
ha731548874 4:45bd9ba6c0da 16 signed short int KONDO2_Positions_Target[KONDO2_MAX_TOTAL];
ha731548874 4:45bd9ba6c0da 17 unsigned char KONDO2_SendOrder[]= {7,9,10};
ha731548874 4:45bd9ba6c0da 18 signed char KONDO2_Directions = 1;
ha731548874 4:45bd9ba6c0da 19 signed char KONDO2_PtrCommands_Set;
ha731548874 4:45bd9ba6c0da 20 signed char KONDO2_ID = 0;
ha731548874 4:45bd9ba6c0da 21 EventQueue KONDO2_queue;
ha731548874 4:45bd9ba6c0da 22 void KONDO2_Positions_Set(double* KONDO2_Degrees)
ha731548874 4:45bd9ba6c0da 23 {
ha731548874 4:45bd9ba6c0da 24 unsigned char id;
ha731548874 4:45bd9ba6c0da 25 for(KONDO2_ID=0;KONDO2_ID < sizeof(KONDO2_SendOrder);KONDO2_ID++)
ha731548874 4:45bd9ba6c0da 26 {
ha731548874 4:45bd9ba6c0da 27 id = KONDO2_SendOrder[KONDO2_ID];
ha731548874 4:45bd9ba6c0da 28 KONDO2_Positions_Target[id] = KONDO2_Degrees[id] * KONDO2_STEP_PER_DEGREE * KONDO2_Directions + KONDO2_Positions_Initial[id];
ha731548874 4:45bd9ba6c0da 29 // wait(0.0008);
ha731548874 4:45bd9ba6c0da 30 }
ha731548874 4:45bd9ba6c0da 31 }
ha731548874 4:45bd9ba6c0da 32 void KONDO2_Interrupt_Tx()
ha731548874 4:45bd9ba6c0da 33 {
ha731548874 4:45bd9ba6c0da 34 while(KONDO2_serial.writeable())
ha731548874 4:45bd9ba6c0da 35 {
ha731548874 4:45bd9ba6c0da 36 KONDO2_serial.putc(KONDO2_Buffer_Tx[KONDO2_Buffer_Tx_Ptr]);
ha731548874 4:45bd9ba6c0da 37 // KONDO2_Buffer_Rx[KONDO2_Buffer_Rx_Ptr] = KONDO2_serial.getc();
ha731548874 4:45bd9ba6c0da 38 KONDO2_Buffer_Tx_Ptr++;
ha731548874 4:45bd9ba6c0da 39 // KONDO2_Buffer_Rx_Ptr++;
ha731548874 4:45bd9ba6c0da 40 if(KONDO2_Buffer_Tx_Ptr==3 && KONDO2_Buffer_Rx_Ptr==3)
ha731548874 4:45bd9ba6c0da 41 KONDO2_Buffer_Tx_Ptr = 0;
ha731548874 4:45bd9ba6c0da 42 // KONDO2_Buffer_Rx_Ptr = 0;
ha731548874 4:45bd9ba6c0da 43 }
ha731548874 4:45bd9ba6c0da 44 }
ha731548874 4:45bd9ba6c0da 45 void KONDO2_Interrupt_Rx()
ha731548874 4:45bd9ba6c0da 46 {
ha731548874 4:45bd9ba6c0da 47 while(KONDO2_serial.readable())
ha731548874 4:45bd9ba6c0da 48 {
ha731548874 4:45bd9ba6c0da 49 // KONDO2_Buffer_Rx_Ptr = 3;
ha731548874 4:45bd9ba6c0da 50 KONDO2_Buffer_Rx[KONDO2_Buffer_Rx_Ptr] = KONDO2_serial.getc();
ha731548874 4:45bd9ba6c0da 51 KONDO2_Buffer_Rx_Ptr++;
ha731548874 4:45bd9ba6c0da 52 if(KONDO2_Buffer_Rx_Ptr==6)
ha731548874 4:45bd9ba6c0da 53 {
ha731548874 4:45bd9ba6c0da 54 KONDO2_Buffer_Rx_Full = true;
ha731548874 4:45bd9ba6c0da 55 KONDO2_Buffer_Rx_Ptr=0;
ha731548874 4:45bd9ba6c0da 56 }
ha731548874 4:45bd9ba6c0da 57 }
ha731548874 4:45bd9ba6c0da 58 }
ha731548874 4:45bd9ba6c0da 59 void KONDO2_update(unsigned char Id, unsigned short int Position)
ha731548874 4:45bd9ba6c0da 60 {
ha731548874 4:45bd9ba6c0da 61 unsigned char id,lo,hi;
ha731548874 4:45bd9ba6c0da 62 id=0x80|Id;
ha731548874 4:45bd9ba6c0da 63 hi=(Position>>7)&0x007F;
ha731548874 4:45bd9ba6c0da 64 lo=Position&0x007F;
ha731548874 4:45bd9ba6c0da 65 NVIC_DisableIRQ(USART2_IRQn);
ha731548874 4:45bd9ba6c0da 66 KONDO2_Buffer_Tx[0] = id;
ha731548874 4:45bd9ba6c0da 67 KONDO2_Buffer_Tx[1] = hi;
ha731548874 4:45bd9ba6c0da 68 KONDO2_Buffer_Tx[2] = lo;
ha731548874 4:45bd9ba6c0da 69 // wait(0.0008);
ha731548874 4:45bd9ba6c0da 70 KONDO2_Buffer_Rx_Full = false;
ha731548874 4:45bd9ba6c0da 71 NVIC_EnableIRQ(USART2_IRQn);
ha731548874 4:45bd9ba6c0da 72 }
ha731548874 4:45bd9ba6c0da 73 void KONDO2_task()
ha731548874 4:45bd9ba6c0da 74 {
ha731548874 4:45bd9ba6c0da 75 signed char id;
ha731548874 4:45bd9ba6c0da 76 signed char i;
ha731548874 4:45bd9ba6c0da 77 for(i=0 ; i < sizeof(KONDO2_SendOrder) ; i++)
ha731548874 4:45bd9ba6c0da 78 {
ha731548874 4:45bd9ba6c0da 79 id = KONDO2_SendOrder[i];
ha731548874 4:45bd9ba6c0da 80 KONDO2_update(id, KONDO2_Positions_Target[id]);
ha731548874 4:45bd9ba6c0da 81 // wait_us(825);
ha731548874 4:45bd9ba6c0da 82 wait_us(650);
ha731548874 4:45bd9ba6c0da 83 }
ha731548874 4:45bd9ba6c0da 84 }
ha731548874 4:45bd9ba6c0da 85 void KONDO2_init()
ha731548874 4:45bd9ba6c0da 86 {
ha731548874 4:45bd9ba6c0da 87 KONDO2_serial.format(8,Serial::Even,1);
ha731548874 4:45bd9ba6c0da 88 KONDO2_serial.baud(115200);
ha731548874 4:45bd9ba6c0da 89 KONDO2_Buffer_Rx_Ptr = 0;
ha731548874 4:45bd9ba6c0da 90 KONDO2_Buffer_Rx_Full = false;
ha731548874 4:45bd9ba6c0da 91 KONDO2_serial.attach(&KONDO2_Interrupt_Rx, Serial::RxIrq);
ha731548874 4:45bd9ba6c0da 92 KONDO2_serial.attach(&KONDO2_Interrupt_Tx, Serial::TxIrq);
ha731548874 4:45bd9ba6c0da 93 NVIC_EnableIRQ(USART2_IRQn);
ha731548874 4:45bd9ba6c0da 94 KONDO2_queue.call_every(10, KONDO2_task);
ha731548874 4:45bd9ba6c0da 95 KONDO2_queue.dispatch();
ha731548874 4:45bd9ba6c0da 96 }