Junjie Wang / Mbed OS a_NYP_humanoid_copy

NYP_Humanoid_robot_FYP_2018

Dependencies:   LSM6DSL

Fork of b_NYP_humanoid by Junjie Wang

kondo3.cpp@5:1faeeab28bd1, 2018-06-05 (annotated)

Committer:
mr_wang
Date:
Tue Jun 05 09:34:33 2018 +0000
Revision:
5:1faeeab28bd1
Parent:
4:99891561a38b 
NYP!!

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