Lin ShengKun / Mbed OS NYP_FYP

FYP

Dependencies:   LSM6DSL

Fork of humanoid by Junjie Wang

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