Junjie Wang / Mbed OS a_NYP_humanoid_copy

NYP_Humanoid_robot_FYP_2018

Dependencies:   LSM6DSL

Fork of b_NYP_humanoid by Junjie Wang

kondo1.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 "kondo1.h"
mr_wang 4:99891561a38b 3 #include "servo.h"
mr_wang 4:99891561a38b 4 #define KONDO1_COMMAND_POSITION_FREE 1
mr_wang 4:99891561a38b 5 #define KONDO1_COMMAND_POSITION_SET 2
mr_wang 4:99891561a38b 6 #define KONDO1_STEP_PER_DEGREE 8000/270
mr_wang 4:99891561a38b 7 RawSerial KONDO1_serial(PC_4,PC_5);
mr_wang 4:99891561a38b 8 unsigned char KONDO1_Buffer_Tx[3];
mr_wang 4:99891561a38b 9 unsigned char KONDO1_Buffer_Tx_Ptr = 0;
mr_wang 4:99891561a38b 10 unsigned char KONDO1_Buffer_Tx_Full = false;
mr_wang 4:99891561a38b 11 unsigned char KONDO1_Buffer_Rx[6];
mr_wang 4:99891561a38b 12 unsigned char KONDO1_Buffer_Rx_Ptr = 3;
mr_wang 4:99891561a38b 13 unsigned char KONDO1_Buffer_Rx_Full = false;
mr_wang 4:99891561a38b 14 signed short int KONDO1_Positions_Initial[] = {7500,7500,7500,7500,7500,7500,7500};
mr_wang 4:99891561a38b 15 signed short int KONDO1_Positions_Target[KONDO1_MAX_TOTAL];
mr_wang 4:99891561a38b 16 //signed short int KONDO1_Positions_Current[KONDO1_MAX_TOTAL]= {7500,7500,7500,7500,7500};
mr_wang 4:99891561a38b 17 unsigned char KONDO1_SendOrder[]= {1,2,3,4,5,6};
mr_wang 4:99891561a38b 18 signed char KONDO1_Directions = 1;
mr_wang 4:99891561a38b 19 signed char KONDO1_ID = 0;
mr_wang 4:99891561a38b 20 signed char KONDO1_PtrCommands_Set;
mr_wang 4:99891561a38b 21 EventQueue KONDO1_queue;
mr_wang 4:99891561a38b 22 /*signed short int KONDO1_Position_Convert(char hi, char lo)
mr_wang 4:99891561a38b 23 {
mr_wang 4:99891561a38b 24 signed short int i = hi;
mr_wang 4:99891561a38b 25 i = i<<7;
mr_wang 4:99891561a38b 26 i = i|lo;
mr_wang 4:99891561a38b 27 return i;
mr_wang 4:99891561a38b 28 }
mr_wang 4:99891561a38b 29 void KONDO1_Position_Current_Set()
mr_wang 4:99891561a38b 30 {
mr_wang 4:99891561a38b 31 unsigned char id;
mr_wang 4:99891561a38b 32 id = KONDO1_Buffer_Rx[0]&0x1F;
mr_wang 4:99891561a38b 33 KONDO1_Positions_Current[id] = KONDO1_Position_Convert(KONDO1_Buffer_Rx[4], KONDO1_Buffer_Rx[5]);
mr_wang 4:99891561a38b 34 }*/
mr_wang 4:99891561a38b 35 void KONDO1_Positions_Set(double* KONDO1_Degrees)
mr_wang 4:99891561a38b 36 {
mr_wang 4:99891561a38b 37 unsigned char id;
mr_wang 4:99891561a38b 38 for(KONDO1_ID=0;KONDO1_ID < sizeof(KONDO1_SendOrder);KONDO1_ID++)
mr_wang 4:99891561a38b 39 {
mr_wang 4:99891561a38b 40 id = KONDO1_SendOrder[KONDO1_ID];
mr_wang 4:99891561a38b 41 // switch(KONDO1_PtrCommands_Set)
mr_wang 4:99891561a38b 42 // {
mr_wang 4:99891561a38b 43 // case KONDO1_COMMAND_POSITION_FREE:
mr_wang 4:99891561a38b 44 // KONDO1_Positions[id]=0;
mr_wang 4:99891561a38b 45 // break;
mr_wang 4:99891561a38b 46 // case KONDO1_COMMAND_POSITION_SET:
mr_wang 4:99891561a38b 47 KONDO1_Positions_Target[id] = KONDO1_Degrees[id] * KONDO1_STEP_PER_DEGREE * KONDO1_Directions + KONDO1_Positions_Initial[id];
mr_wang 4:99891561a38b 48 // break;
mr_wang 4:99891561a38b 49 // case KONDO1_COMMAND_DEGREE_SET:
mr_wang 4:99891561a38b 50 // KONDO1_degrees_Target[id] = (KONDO1_Positions_Target[id] - KONDO1_Positions_Initial[id])/ (KONDO1_STEP_PER_DEGREE * KONDO1_Directions);
mr_wang 4:99891561a38b 51 // break;
mr_wang 4:99891561a38b 52 //wait(0.000625);
mr_wang 4:99891561a38b 53 }
mr_wang 4:99891561a38b 54 }
mr_wang 4:99891561a38b 55 void KONDO1_Interrupt_Tx()
mr_wang 4:99891561a38b 56 {
mr_wang 4:99891561a38b 57 while(KONDO1_serial.writeable())
mr_wang 4:99891561a38b 58 {
mr_wang 4:99891561a38b 59 KONDO1_serial.putc(KONDO1_Buffer_Tx[KONDO1_Buffer_Tx_Ptr]);
mr_wang 4:99891561a38b 60 KONDO1_Buffer_Tx_Ptr++;
mr_wang 4:99891561a38b 61 if(KONDO1_Buffer_Tx_Ptr==3)
mr_wang 4:99891561a38b 62 KONDO1_Buffer_Tx_Ptr = 0;
mr_wang 4:99891561a38b 63 }
mr_wang 4:99891561a38b 64 }
mr_wang 4:99891561a38b 65 void KONDO1_Interrupt_Rx()
mr_wang 4:99891561a38b 66 {
mr_wang 4:99891561a38b 67 while(KONDO1_serial.readable())
mr_wang 4:99891561a38b 68 {
mr_wang 4:99891561a38b 69 KONDO1_Buffer_Rx[KONDO1_Buffer_Rx_Ptr] = KONDO1_serial.getc();
mr_wang 4:99891561a38b 70 KONDO1_Buffer_Rx_Ptr++;
mr_wang 4:99891561a38b 71 if(KONDO1_Buffer_Rx_Ptr==6)
mr_wang 4:99891561a38b 72 {
mr_wang 4:99891561a38b 73 KONDO1_Buffer_Rx_Full = true;
mr_wang 4:99891561a38b 74 KONDO1_Buffer_Rx_Ptr=0;
mr_wang 4:99891561a38b 75 // KONDO1_Position_Current_Set();
mr_wang 4:99891561a38b 76 }
mr_wang 4:99891561a38b 77 }
mr_wang 4:99891561a38b 78 }
mr_wang 4:99891561a38b 79 void KONDO1_update(unsigned char Id, unsigned short int Position)
mr_wang 4:99891561a38b 80 {
mr_wang 4:99891561a38b 81 unsigned char id,lo,hi;
mr_wang 4:99891561a38b 82 id=0x80|Id;
mr_wang 4:99891561a38b 83 hi=(Position>>7)&0x007F;
mr_wang 4:99891561a38b 84 lo=Position&0x007F;
mr_wang 4:99891561a38b 85 NVIC_DisableIRQ(USART3_IRQn);
mr_wang 4:99891561a38b 86 KONDO1_Buffer_Tx[0] = id;// KONDO1_Buffer_Rx[0] = KONDO1_serial.getc();
mr_wang 4:99891561a38b 87 KONDO1_Buffer_Tx[1] = hi;// KONDO1_Buffer_Rx[1] = KONDO1_serial.getc();
mr_wang 4:99891561a38b 88 KONDO1_Buffer_Tx[2] = lo;// KONDO1_Buffer_Rx[2] = KONDO1_serial.getc();
mr_wang 4:99891561a38b 89 // wait(0.0007);
mr_wang 4:99891561a38b 90 KONDO1_Buffer_Rx_Full = false;
mr_wang 4:99891561a38b 91 NVIC_EnableIRQ(USART3_IRQn);
mr_wang 4:99891561a38b 92 }
mr_wang 4:99891561a38b 93 void KONDO1_task()
mr_wang 4:99891561a38b 94 {
mr_wang 4:99891561a38b 95 signed char id;
mr_wang 4:99891561a38b 96 signed char i;
mr_wang 4:99891561a38b 97 for(i=0 ; i < sizeof(KONDO1_SendOrder) ; i++)
mr_wang 4:99891561a38b 98 {
mr_wang 4:99891561a38b 99 id = KONDO1_SendOrder[i];
mr_wang 4:99891561a38b 100 // if(id==0)KONDO1_update(id, 0);else
mr_wang 4:99891561a38b 101 KONDO1_update(id, KONDO1_Positions_Target[id]);
mr_wang 4:99891561a38b 102 wait_us(650); //650 1000
mr_wang 4:99891561a38b 103 // printf("%d=%d\r\n",id,KONDO1_Positions_Current[id]);
mr_wang 4:99891561a38b 104 // 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]);
mr_wang 4:99891561a38b 105 }
mr_wang 4:99891561a38b 106 }
mr_wang 4:99891561a38b 107 void KONDO1_init()
mr_wang 4:99891561a38b 108 {
mr_wang 4:99891561a38b 109 KONDO1_serial.format(8,Serial::Even,1);
mr_wang 4:99891561a38b 110 KONDO1_serial.baud(115200);
mr_wang 4:99891561a38b 111 KONDO1_Buffer_Rx_Ptr = 0;
mr_wang 4:99891561a38b 112 KONDO1_Buffer_Rx_Full = false;
mr_wang 4:99891561a38b 113 KONDO1_serial.attach(&KONDO1_Interrupt_Rx, Serial::RxIrq);
mr_wang 4:99891561a38b 114 KONDO1_serial.attach(&KONDO1_Interrupt_Tx, Serial::TxIrq);
mr_wang 4:99891561a38b 115 NVIC_EnableIRQ(USART3_IRQn);
mr_wang 4:99891561a38b 116 KONDO1_queue.call_every(10, KONDO1_task); //1000 0.1 1
mr_wang 4:99891561a38b 117 KONDO1_queue.dispatch();
mr_wang 4:99891561a38b 118 }