servo.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 DigitalOut led3(LED3);
00004 RawSerial motor(PC_4,PC_5);
00005 unsigned char tx_buffer[3];
00006 unsigned char tx_buffer_ptr = 0;
00007 unsigned char tx_buffer_full = false;
00008 unsigned char rx_buffer[6];
00009 unsigned char rx_buffer_ptr = 0;
00010 unsigned char rx_buffer_full = false;
00011 signed short int motorcp[] = {8500,8500};
00012 unsigned short int Position;
00013 int MotorID;
00014 void Tx_interrupt();
00015 void Rx_interrupt();
00016 //EventQueue queue;
00017 //Thread t;
00018 
00019 void motor_update(unsigned char Id, unsigned short int Position)
00020 {
00021     unsigned char id,lo,hi;
00022     id=0x80|Id;
00023     hi=(Position>>7)&0x007F;
00024     lo=Position&0x007F;
00025     NVIC_DisableIRQ(USART3_IRQn);
00026     tx_buffer[0] = id;
00027     tx_buffer[1] = hi;
00028     tx_buffer[2] = lo;
00029     NVIC_EnableIRQ(USART3_IRQn);
00030 }
00031 void SERVO_task() 
00032 {
00033     led3 = !led3;
00034     for(MotorID=0;MotorID<=1;MotorID++)
00035     {
00036         motor_update(MotorID,motorcp[MotorID]);
00037         wait_us(900);
00038     }
00039 }
00040 void SERVO_init() 
00041 {
00042     motor.format(8,Serial::Even,1);
00043     motor.baud(115200);
00044     rx_buffer_ptr = 0;
00045     rx_buffer_full = false;
00046     motor.attach(&Rx_interrupt, Serial::RxIrq);
00047     motor.attach(&Tx_interrupt, Serial::TxIrq);
00048     NVIC_EnableIRQ(USART3_IRQn);
00049     //t.start(callback(&queue, &EventQueue::dispatch_forever));
00050     //queue.call_every(1000, SERVO_init);
00051 /*
00052     while(1)
00053     {
00054         motorcp[0]=7500;
00055         motorcp[1]=7500;
00056         Thread::wait(1000);
00057         motorcp[0]=8500;
00058         motorcp[1]=8500;
00059         Thread::wait(1000);
00060     }
00061 */
00062 }
00063 
00064 void Tx_interrupt() 
00065 {
00066     while(motor.writeable())
00067     {    
00068         motor.putc(tx_buffer[tx_buffer_ptr]);
00069         tx_buffer_ptr++;
00070         if(tx_buffer_ptr==3)
00071               tx_buffer_ptr = 0;
00072     }
00073     return;
00074 }
00075 void Rx_interrupt()
00076 {
00077     while(motor.readable())
00078     {
00079         rx_buffer[rx_buffer_ptr] = motor.getc();
00080         rx_buffer_ptr++;
00081         if(rx_buffer_ptr==6)
00082         {
00083             rx_buffer_full = true;
00084             rx_buffer_ptr=0;
00085         }
00086     }
00087 } 
00088