Lin ShengKun / Mbed OS NYP_FYP

FYP

Dependencies:   LSM6DSL

Fork of humanoid by Junjie Wang

servo.cpp@2:7d574b1ab3cd, 2018-05-02 (annotated)

Committer:
ha731548874
Date:
Wed May 02 03:33:15 2018 +0000
Revision:
2:7d574b1ab3cd
Parent:
1:cde16b5e604d
Child:
3:1345f959c490
together

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 DigitalOut l1(LED1);
ha731548874 1:cde16b5e604d 4 DigitalOut l2(LED2);
ha731548874 1:cde16b5e604d 5 DigitalOut l3(LED3);
ha731548874 1:cde16b5e604d 6 DigitalOut l4(LED4);
ha731548874 1:cde16b5e604d 7 RawSerial motor(PC_4,PC_5);
ha731548874 1:cde16b5e604d 8 unsigned char tx_buffer[3];
ha731548874 1:cde16b5e604d 9 unsigned char tx_buffer_ptr = 0;
ha731548874 1:cde16b5e604d 10 unsigned char tx_buffer_full = false;
ha731548874 1:cde16b5e604d 11 unsigned char rx_buffer[6];
ha731548874 1:cde16b5e604d 12 unsigned char rx_buffer_ptr = 0;
ha731548874 1:cde16b5e604d 13 unsigned char rx_buffer_full = false;
ha731548874 1:cde16b5e604d 14 signed short int motorcp[] = {8500,8500};
ha731548874 1:cde16b5e604d 15 unsigned short int Position;
ha731548874 1:cde16b5e604d 16 int MotorID;
ha731548874 1:cde16b5e604d 17 void Tx_interrupt();
ha731548874 1:cde16b5e604d 18 void Rx_interrupt();
ha731548874 2:7d574b1ab3cd 19 //EventQueue queue;
ha731548874 2:7d574b1ab3cd 20 //Thread t;
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 2:7d574b1ab3cd 33 void SERVO_task()
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 2:7d574b1ab3cd 41 void SERVO_init()
ha731548874 1:cde16b5e604d 42 {
ha731548874 2:7d574b1ab3cd 43 l1 = !l1;
ha731548874 1:cde16b5e604d 44 motor.format(8,Serial::Even,1);
ha731548874 1:cde16b5e604d 45 motor.baud(115200);
ha731548874 1:cde16b5e604d 46 rx_buffer_ptr = 0;
ha731548874 1:cde16b5e604d 47 rx_buffer_full = false;
ha731548874 1:cde16b5e604d 48 motor.attach(&Rx_interrupt, Serial::RxIrq);
ha731548874 1:cde16b5e604d 49 motor.attach(&Tx_interrupt, Serial::TxIrq);
ha731548874 1:cde16b5e604d 50 NVIC_EnableIRQ(USART3_IRQn);
ha731548874 2:7d574b1ab3cd 51 // t.start(callback(&queue, &EventQueue::dispatch_forever));
ha731548874 2:7d574b1ab3cd 52 //queue.call_every(1000, SERVO_init);
ha731548874 1:cde16b5e604d 53 while(1)
ha731548874 1:cde16b5e604d 54 {
ha731548874 1:cde16b5e604d 55 motorcp[0]=7500;
ha731548874 1:cde16b5e604d 56 motorcp[1]=7500;
ha731548874 1:cde16b5e604d 57 Thread::wait(1000);
ha731548874 1:cde16b5e604d 58 motorcp[0]=8500;
ha731548874 1:cde16b5e604d 59 motorcp[1]=8500;
ha731548874 1:cde16b5e604d 60 Thread::wait(1000);
ha731548874 1:cde16b5e604d 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