Lin ShengKun / Mbed 2 deprecated A_test_motor

A_test_motor

Dependencies:   mbed-rtos mbed

Fork of ex_1 by Lin ShengKun

main.cpp@0:353bcc1d30da, 2018-05-04 (annotated)

Committer:
ha731548874
Date:
Fri May 04 10:37:35 2018 +0000
Revision:
0:353bcc1d30da
NYP

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ha731548874 0:353bcc1d30da 1 #include "mbed.h"
ha731548874 0:353bcc1d30da 2 #include "rtos.h"
ha731548874 0:353bcc1d30da 3 DigitalOut l1(LED1);
ha731548874 0:353bcc1d30da 4 DigitalOut l2(LED2);
ha731548874 0:353bcc1d30da 5 DigitalOut l3(LED3);
ha731548874 0:353bcc1d30da 6 DigitalOut l4(LED4);
ha731548874 0:353bcc1d30da 7 RawSerial kondo2(PC_1,PC_0);
ha731548874 0:353bcc1d30da 8 unsigned char tx_buffer[3];
ha731548874 0:353bcc1d30da 9 unsigned char tx_buffer_ptr = 0;
ha731548874 0:353bcc1d30da 10 unsigned char tx_buffer_full = false;
ha731548874 0:353bcc1d30da 11 unsigned char rx_buffer[6];
ha731548874 0:353bcc1d30da 12 unsigned char rx_buffer_ptr = 0;
ha731548874 0:353bcc1d30da 13 unsigned char rx_buffer_full = false;
ha731548874 0:353bcc1d30da 14 signed short int motorcp[] = {8500,8500};
ha731548874 0:353bcc1d30da 15 int MotorID;
ha731548874 0:353bcc1d30da 16 Thread servo;
ha731548874 0:353bcc1d30da 17 void KONDO2_interrupt_tx();
ha731548874 0:353bcc1d30da 18 void KONDO2_interrupt_rx();
ha731548874 0:353bcc1d30da 19 void kondo2_update(unsigned char Id, unsigned short int Position)
ha731548874 0:353bcc1d30da 20 {
ha731548874 0:353bcc1d30da 21 unsigned char id,lo,hi;
ha731548874 0:353bcc1d30da 22 id=0x80|Id;
ha731548874 0:353bcc1d30da 23 hi=(Position>>7)&0x007F;
ha731548874 0:353bcc1d30da 24 lo=Position&0x007F;
ha731548874 0:353bcc1d30da 25 // NVIC_DisableIRQ(USART3_IRQn);
ha731548874 0:353bcc1d30da 26 tx_buffer[0] = id;
ha731548874 0:353bcc1d30da 27 tx_buffer[1] = hi;
ha731548874 0:353bcc1d30da 28 tx_buffer[2] = lo;
ha731548874 0:353bcc1d30da 29 // NVIC_EnableIRQ(USART3_IRQn);
ha731548874 0:353bcc1d30da 30 }
ha731548874 0:353bcc1d30da 31 void KONDO2_task()
ha731548874 0:353bcc1d30da 32 {
ha731548874 0:353bcc1d30da 33 for(MotorID=0;MotorID<=1;MotorID++)
ha731548874 0:353bcc1d30da 34 {
ha731548874 0:353bcc1d30da 35 kondo2_update(MotorID,motorcp[MotorID]);
ha731548874 0:353bcc1d30da 36 wait_us(900);
ha731548874 0:353bcc1d30da 37 }
ha731548874 0:353bcc1d30da 38 }
ha731548874 0:353bcc1d30da 39 void KONDO2_init()
ha731548874 0:353bcc1d30da 40 {
ha731548874 0:353bcc1d30da 41 kondo2.format(8,Serial::Even,1);
ha731548874 0:353bcc1d30da 42 kondo2.baud(115200);
ha731548874 0:353bcc1d30da 43 rx_buffer_ptr = 0;
ha731548874 0:353bcc1d30da 44 rx_buffer_full = false;
ha731548874 0:353bcc1d30da 45 kondo2.attach(&KONDO2_interrupt_rx, Serial::RxIrq);
ha731548874 0:353bcc1d30da 46 kondo2.attach(&KONDO2_interrupt_tx, Serial::TxIrq);
ha731548874 0:353bcc1d30da 47 // NVIC_EnableIRQ(USART3_IRQn);
ha731548874 0:353bcc1d30da 48 RtosTimer motorposition(KONDO2_task);
ha731548874 0:353bcc1d30da 49 motorposition.start(10);
ha731548874 0:353bcc1d30da 50 while(1)
ha731548874 0:353bcc1d30da 51 {
ha731548874 0:353bcc1d30da 52 motorcp[0]=7500;
ha731548874 0:353bcc1d30da 53 motorcp[1]=7500;
ha731548874 0:353bcc1d30da 54 Thread::wait(1000);
ha731548874 0:353bcc1d30da 55 motorcp[0]=8500;
ha731548874 0:353bcc1d30da 56 motorcp[1]=8500;
ha731548874 0:353bcc1d30da 57 Thread::wait(1000);
ha731548874 0:353bcc1d30da 58 }
ha731548874 0:353bcc1d30da 59 }
ha731548874 0:353bcc1d30da 60 int main()
ha731548874 0:353bcc1d30da 61 {
ha731548874 0:353bcc1d30da 62 servo.start(KONDO2_init);
ha731548874 0:353bcc1d30da 63 while(1)
ha731548874 0:353bcc1d30da 64 {
ha731548874 0:353bcc1d30da 65 l1 = !l1;
ha731548874 0:353bcc1d30da 66 Thread::wait(1000);
ha731548874 0:353bcc1d30da 67 }
ha731548874 0:353bcc1d30da 68 }
ha731548874 0:353bcc1d30da 69 void KONDO2_interrupt_tx()
ha731548874 0:353bcc1d30da 70 {
ha731548874 0:353bcc1d30da 71 while(kondo2.writeable())
ha731548874 0:353bcc1d30da 72 {
ha731548874 0:353bcc1d30da 73 kondo2.putc(tx_buffer[tx_buffer_ptr]);
ha731548874 0:353bcc1d30da 74 tx_buffer_ptr++;
ha731548874 0:353bcc1d30da 75 if(tx_buffer_ptr==3)
ha731548874 0:353bcc1d30da 76 tx_buffer_ptr = 0;
ha731548874 0:353bcc1d30da 77 }
ha731548874 0:353bcc1d30da 78 return;
ha731548874 0:353bcc1d30da 79 }
ha731548874 0:353bcc1d30da 80 void KONDO2_interrupt_rx()
ha731548874 0:353bcc1d30da 81 {
ha731548874 0:353bcc1d30da 82 while(kondo2.readable())
ha731548874 0:353bcc1d30da 83 {
ha731548874 0:353bcc1d30da 84 rx_buffer[rx_buffer_ptr] = kondo2.getc();
ha731548874 0:353bcc1d30da 85 rx_buffer_ptr++;
ha731548874 0:353bcc1d30da 86 if(rx_buffer_ptr==6)
ha731548874 0:353bcc1d30da 87 {
ha731548874 0:353bcc1d30da 88 rx_buffer_full = true;
ha731548874 0:353bcc1d30da 89 rx_buffer_ptr=0;
ha731548874 0:353bcc1d30da 90 }
ha731548874 0:353bcc1d30da 91 }
ha731548874 0:353bcc1d30da 92 }