Junjie Wang / Mbed OS a_NYP_humanoid_copy

NYP_Humanoid_robot_FYP_2018

Dependencies:   LSM6DSL

Fork of b_NYP_humanoid by Junjie Wang

kondo2.cpp@4:99891561a38b, 2018-05-25 (annotated)

Committer:
mr_wang
Date:
Fri May 25 09:00:15 2018 +0000
Revision:
4:99891561a38b
NYP_DAD

Who changed what in which revision?

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