Lin ShengKun / Mbed OS NYP_FYP

FYP

Dependencies:   LSM6DSL

Fork of humanoid by Junjie Wang

servo.cpp@1:cde16b5e604d, 2018-04-30 (annotated)

Committer:
ha731548874
Date:
Mon Apr 30 11:23:06 2018 +0000
Revision:
1:cde16b5e604d
Child:
2:7d574b1ab3cd
hjbjk

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ha731548874 1:cde16b5e604d 1 #include "mbed.h"
ha731548874 1:cde16b5e604d 2 #include "rtos.h"
ha731548874 1:cde16b5e604d 3
ha731548874 1:cde16b5e604d 4 DigitalOut l1(LED1);
ha731548874 1:cde16b5e604d 5 DigitalOut l2(LED2);
ha731548874 1:cde16b5e604d 6 DigitalOut l3(LED3);
ha731548874 1:cde16b5e604d 7 DigitalOut l4(LED4);
ha731548874 1:cde16b5e604d 8 RawSerial motor(PC_4,PC_5);
ha731548874 1:cde16b5e604d 9 unsigned char tx_buffer[3];
ha731548874 1:cde16b5e604d 10 unsigned char tx_buffer_ptr = 0;
ha731548874 1:cde16b5e604d 11 unsigned char tx_buffer_full = false;
ha731548874 1:cde16b5e604d 12 unsigned char rx_buffer[6];
ha731548874 1:cde16b5e604d 13 unsigned char rx_buffer_ptr = 0;
ha731548874 1:cde16b5e604d 14 unsigned char rx_buffer_full = false;
ha731548874 1:cde16b5e604d 15 signed short int motorcp[] = {8500,8500};
ha731548874 1:cde16b5e604d 16 unsigned short int Position;
ha731548874 1:cde16b5e604d 17 int MotorID;
ha731548874 1:cde16b5e604d 18 void Tx_interrupt();
ha731548874 1:cde16b5e604d 19 void Rx_interrupt();
ha731548874 1:cde16b5e604d 20
ha731548874 1:cde16b5e604d 21 void motor_update(unsigned char Id, unsigned short int Position)
ha731548874 1:cde16b5e604d 22 {
ha731548874 1:cde16b5e604d 23 unsigned char id,lo,hi;
ha731548874 1:cde16b5e604d 24 id=0x80|Id;
ha731548874 1:cde16b5e604d 25 hi=(Position>>7)&0x007F;
ha731548874 1:cde16b5e604d 26 lo=Position&0x007F;
ha731548874 1:cde16b5e604d 27 NVIC_DisableIRQ(USART3_IRQn);
ha731548874 1:cde16b5e604d 28 tx_buffer[0] = id;
ha731548874 1:cde16b5e604d 29 tx_buffer[1] = hi;
ha731548874 1:cde16b5e604d 30 tx_buffer[2] = lo;
ha731548874 1:cde16b5e604d 31 NVIC_EnableIRQ(USART3_IRQn);
ha731548874 1:cde16b5e604d 32 }
ha731548874 1:cde16b5e604d 33 void SERVO_init()
ha731548874 1:cde16b5e604d 34 {
ha731548874 1:cde16b5e604d 35 for(MotorID=0;MotorID<=1;MotorID++)
ha731548874 1:cde16b5e604d 36 {
ha731548874 1:cde16b5e604d 37 motor_update(MotorID,motorcp[MotorID]);
ha731548874 1:cde16b5e604d 38 wait_us(900);
ha731548874 1:cde16b5e604d 39 }
ha731548874 1:cde16b5e604d 40 }
ha731548874 1:cde16b5e604d 41 void SERVO_task()
ha731548874 1:cde16b5e604d 42 {
ha731548874 1:cde16b5e604d 43 motor.format(8,Serial::Even,1);
ha731548874 1:cde16b5e604d 44 motor.baud(115200);
ha731548874 1:cde16b5e604d 45 rx_buffer_ptr = 0;
ha731548874 1:cde16b5e604d 46 rx_buffer_full = false;
ha731548874 1:cde16b5e604d 47 motor.attach(&Rx_interrupt, Serial::RxIrq);
ha731548874 1:cde16b5e604d 48 motor.attach(&Tx_interrupt, Serial::TxIrq);
ha731548874 1:cde16b5e604d 49 NVIC_EnableIRQ(USART3_IRQn);
ha731548874 1:cde16b5e604d 50 while(1)
ha731548874 1:cde16b5e604d 51 {
ha731548874 1:cde16b5e604d 52 motorcp[0]=7500;
ha731548874 1:cde16b5e604d 53 motorcp[1]=7500;
ha731548874 1:cde16b5e604d 54 Thread::wait(1000);
ha731548874 1:cde16b5e604d 55 motorcp[0]=8500;
ha731548874 1:cde16b5e604d 56 motorcp[1]=8500;
ha731548874 1:cde16b5e604d 57 Thread::wait(1000);
ha731548874 1:cde16b5e604d 58 }
ha731548874 1:cde16b5e604d 59 }
ha731548874 1:cde16b5e604d 60
ha731548874 1:cde16b5e604d 61 void Tx_interrupt()
ha731548874 1:cde16b5e604d 62 {
ha731548874 1:cde16b5e604d 63 while(motor.writeable())
ha731548874 1:cde16b5e604d 64 {
ha731548874 1:cde16b5e604d 65 motor.putc(tx_buffer[tx_buffer_ptr]);
ha731548874 1:cde16b5e604d 66 tx_buffer_ptr++;
ha731548874 1:cde16b5e604d 67 if(tx_buffer_ptr==3)
ha731548874 1:cde16b5e604d 68 tx_buffer_ptr = 0;
ha731548874 1:cde16b5e604d 69 }
ha731548874 1:cde16b5e604d 70 return;
ha731548874 1:cde16b5e604d 71 }
ha731548874 1:cde16b5e604d 72 void Rx_interrupt()
ha731548874 1:cde16b5e604d 73 {
ha731548874 1:cde16b5e604d 74 while(motor.readable())
ha731548874 1:cde16b5e604d 75 {
ha731548874 1:cde16b5e604d 76 rx_buffer[rx_buffer_ptr] = motor.getc();
ha731548874 1:cde16b5e604d 77 rx_buffer_ptr++;
ha731548874 1:cde16b5e604d 78 if(rx_buffer_ptr==6)
ha731548874 1:cde16b5e604d 79 {
ha731548874 1:cde16b5e604d 80 rx_buffer_full = true;
ha731548874 1:cde16b5e604d 81 rx_buffer_ptr=0;
ha731548874 1:cde16b5e604d 82 }
ha731548874 1:cde16b5e604d 83 }
ha731548874 1:cde16b5e604d 84 }
ha731548874 1:cde16b5e604d 85