main.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 kondo2(PC_1,PC_0);
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 int MotorID;
00016 Thread servo;
00017 void KONDO2_interrupt_tx();
00018 void KONDO2_interrupt_rx();
00019 void kondo2_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 KONDO2_task() 
00032 {
00033     for(MotorID=0;MotorID<=1;MotorID++)
00034     {
00035         kondo2_update(MotorID,motorcp[MotorID]);
00036         wait_us(900);
00037     }
00038 }
00039 void KONDO2_init() 
00040 {
00041     kondo2.format(8,Serial::Even,1);
00042     kondo2.baud(115200);
00043     rx_buffer_ptr = 0;
00044     rx_buffer_full = false;
00045     kondo2.attach(&KONDO2_interrupt_rx, Serial::RxIrq);
00046     kondo2.attach(&KONDO2_interrupt_tx, Serial::TxIrq);
00047 //    NVIC_EnableIRQ(USART3_IRQn);
00048     RtosTimer motorposition(KONDO2_task);
00049     motorposition.start(10);
00050     while(1)
00051     {
00052         motorcp[0]=7500;
00053         motorcp[1]=7500;
00054         Thread::wait(1000);
00055         motorcp[0]=8500;
00056         motorcp[1]=8500;
00057         Thread::wait(1000);
00058     }
00059 }
00060 int main()
00061 {
00062     servo.start(KONDO2_init);
00063     while(1)
00064     {
00065         l1 = !l1;
00066         Thread::wait(1000);
00067     }
00068 }
00069 void KONDO2_interrupt_tx() 
00070 {
00071     while(kondo2.writeable())
00072     {    
00073         kondo2.putc(tx_buffer[tx_buffer_ptr]);
00074         tx_buffer_ptr++;
00075         if(tx_buffer_ptr==3)
00076               tx_buffer_ptr = 0;
00077     }
00078     return;
00079 }
00080 void KONDO2_interrupt_rx()
00081 {
00082     while(kondo2.readable())
00083     {
00084         rx_buffer[rx_buffer_ptr] = kondo2.getc();
00085         rx_buffer_ptr++;
00086         if(rx_buffer_ptr==6)
00087         {
00088             rx_buffer_full = true;
00089             rx_buffer_ptr=0;
00090         }
00091     }
00092 }