Lin ShengKun
A_test_motor
Fork of
ex_1
by 
Lin ShengKun
Revision 0:353bcc1d30da, committed 2018-05-04
- Comitter:
- ha731548874
- Date:
- Fri May 04 10:37:35 2018 +0000
- Commit message:
- NYP
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri May 04 10:37:35 2018 +0000
@@ -0,0 +1,92 @@
+#include "mbed.h"
+#include "rtos.h"
+DigitalOut l1(LED1);
+DigitalOut l2(LED2);
+DigitalOut l3(LED3);
+DigitalOut l4(LED4);
+RawSerial kondo2(PC_1,PC_0);
+unsigned char tx_buffer[3];
+unsigned char tx_buffer_ptr = 0;
+unsigned char tx_buffer_full = false;
+unsigned char rx_buffer[6];
+unsigned char rx_buffer_ptr = 0;
+unsigned char rx_buffer_full = false;
+signed short int motorcp[] = {8500,8500};
+int MotorID;
+Thread servo;
+void KONDO2_interrupt_tx();
+void KONDO2_interrupt_rx();
+void kondo2_update(unsigned char Id, unsigned short int Position)
+{
+ unsigned char id,lo,hi;
+ id=0x80|Id;
+ hi=(Position>>7)&0x007F;
+ lo=Position&0x007F;
+// NVIC_DisableIRQ(USART3_IRQn);
+ tx_buffer[0] = id;
+ tx_buffer[1] = hi;
+ tx_buffer[2] = lo;
+// NVIC_EnableIRQ(USART3_IRQn);
+}
+void KONDO2_task()
+{
+ for(MotorID=0;MotorID<=1;MotorID++)
+ {
+ kondo2_update(MotorID,motorcp[MotorID]);
+ wait_us(900);
+ }
+}
+void KONDO2_init()
+{
+ kondo2.format(8,Serial::Even,1);
+ kondo2.baud(115200);
+ rx_buffer_ptr = 0;
+ rx_buffer_full = false;
+ kondo2.attach(&KONDO2_interrupt_rx, Serial::RxIrq);
+ kondo2.attach(&KONDO2_interrupt_tx, Serial::TxIrq);
+// NVIC_EnableIRQ(USART3_IRQn);
+ RtosTimer motorposition(KONDO2_task);
+ motorposition.start(10);
+ while(1)
+ {
+ motorcp[0]=7500;
+ motorcp[1]=7500;
+ Thread::wait(1000);
+ motorcp[0]=8500;
+ motorcp[1]=8500;
+ Thread::wait(1000);
+ }
+}
+int main()
+{
+ servo.start(KONDO2_init);
+ while(1)
+ {
+ l1 = !l1;
+ Thread::wait(1000);
+ }
+}
+void KONDO2_interrupt_tx()
+{
+ while(kondo2.writeable())
+ {
+ kondo2.putc(tx_buffer[tx_buffer_ptr]);
+ tx_buffer_ptr++;
+ if(tx_buffer_ptr==3)
+ tx_buffer_ptr = 0;
+ }
+ return;
+}
+void KONDO2_interrupt_rx()
+{
+ while(kondo2.readable())
+ {
+ rx_buffer[rx_buffer_ptr] = kondo2.getc();
+ rx_buffer_ptr++;
+ if(rx_buffer_ptr==6)
+ {
+ rx_buffer_full = true;
+ rx_buffer_ptr=0;
+ }
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-rtos.lib Fri May 04 10:37:35 2018 +0000 @@ -0,0 +1,1 @@ +http://os.mbed.com/users/mbed_official/code/mbed-rtos/#5713cbbdb706
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri May 04 10:37:35 2018 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/mbed_official/code/mbed/builds/b484a57bc302 \ No newline at end of file