servo.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 DigitalOut l1(LED1);
00004 DigitalOut l2(LED2);
00005 DigitalOut l3(LED3);
00006 DigitalOut l4(LED4);
00007 RawSerial motor(PC_4,PC_5);
00008 unsigned char tx_buffer[3];
00009 unsigned char tx_buffer_ptr = 0;
00010 unsigned char tx_buffer_full = false;
00011 unsigned char rx_buffer[6];
00012 unsigned char rx_buffer_ptr = 0;
00013 unsigned char rx_buffer_full = false;
00014 signed short int motorcp[] = {8500,8500};
00015 unsigned short int Position;
00016 int MotorID;
00017 void Tx_interrupt();
00018 void Rx_interrupt();
00019 //EventQueue queue;
00020 //Thread t;
00021 void motor_update(unsigned char Id, unsigned short int Position)
00022 {
00023     unsigned char id,lo,hi;
00024     id=0x80|Id;
00025     hi=(Position>>7)&0x007F;
00026     lo=Position&0x007F;
00027     NVIC_DisableIRQ(USART3_IRQn);
00028     tx_buffer[0] = id;
00029     tx_buffer[1] = hi;
00030     tx_buffer[2] = lo;
00031     NVIC_EnableIRQ(USART3_IRQn);
00032 }
00033 void SERVO_task() 
00034 {
00035     for(MotorID=0;MotorID<=1;MotorID++)
00036     {
00037         motor_update(MotorID,motorcp[MotorID]);
00038         wait_us(900);
00039     }
00040 }
00041 void SERVO_init() 
00042 {
00043     l1 = !l1;
00044     motor.format(8,Serial::Even,1);
00045     motor.baud(115200);
00046     rx_buffer_ptr = 0;
00047     rx_buffer_full = false;
00048     motor.attach(&Rx_interrupt, Serial::RxIrq);
00049     motor.attach(&Tx_interrupt, Serial::TxIrq);
00050     NVIC_EnableIRQ(USART3_IRQn);
00051 //    t.start(callback(&queue, &EventQueue::dispatch_forever));
00052     //queue.call_every(1000, SERVO_init);
00053     while(1)
00054     {
00055         motorcp[0]=7500;
00056         motorcp[1]=7500;
00057         Thread::wait(1000);
00058         motorcp[0]=8500;
00059         motorcp[1]=8500;
00060         Thread::wait(1000);
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