Junjie Wang / Mbed OS a_NYP_humanoid_copy

NYP_Humanoid_robot_FYP_2018

Dependencies:   LSM6DSL

Fork of b_NYP_humanoid by Junjie Wang

servo.cpp@3:1345f959c490, 2018-05-02 (annotated)

Committer:
mr_wang
Date:
Wed May 02 07:49:13 2018 +0000
Revision:
3:1345f959c490
Parent:
2:7d574b1ab3cd
Child:
4:99891561a38b
ahah

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