新型モータードライバのメインプログラム
Dependencies: mbed
main.cpp
- Committer:
- SES01
- Date:
- 2016-06-24
- Revision:
- 4:614e752487cc
- Parent:
- 3:65eaf88bf278
File content as of revision 4:614e752487cc:
#include "mbed.h" //重要受信データのマスク #define LEAD_PACKET_MASK 0b10000000 #define PACKET_ADDRESS_MASK 0b00011111 #define PACKET_MODE_MASK 0b01100000 #define PACKET_DATA_MASK 0b01111111 //モータードライバの動作モード #define ORDER_FOLLOWING_MODE 1 #define ENCODER_FOLLOWING_MODE 2 #define SINGLE_ORDER_MODE 3 //単命令を受信した後は必ずsendback //単命令動作の種類 #define S_ORDER_FREQUENCY 0 #define S_ORDER_SEND_POWER 1 #define S_ORDER_LINEAR_AUTOCHANGE 2 #define S_ORDER_CERTAIN 3 #define S_ORDER_SEND_ENCODER 4 #define F_MD 15000 //PWMキャリア周波数設定 /* * High_A High_B * ┃ ┃ * ┃ ┃ * ┣━━━┫ * ┃ ┃ * ┃ ┃ * Low_A Low_B */ Serial comPort(dp16, dp15); DigitalOut driverEnable(dp17); DigitalOut LED(LED1); DigitalOut check_reciveData(LED2); BusIn switch5(dp26, dp25, dp18, dp6, dp4); //dp5 = ハイサイドPWM1, dp27 = ハイサイドPWM2 (オープンドレイン) //dp1 = ローサイドPWM1, dp2 = ローサイドPWM2 PwmOut Low_A(dp1); PwmOut Low_B(dp2); DigitalOut High_A(dp5); DigitalOut High_B(dp27); Timer timer; AnalogIn ain(dp9); //PortIn encoder(Port0, 0b111); //グローバル変数 uint8_t lastRXdata[3] = {0}; uint8_t address = 0; uint8_t mode = 0; uint8_t nowOrderAddress = 0; uint8_t dataNumber = 0; uint8_t sendPowerFlag = 0; uint8_t orderType = 0; uint8_t Ldir = 1; int FP_MD; void orderFollowManager(uint8_t reciveData, uint8_t dataNumber){ switch (dataNumber) { case 2: break; case 3: //comPort.printf("%d %d\n", lastRXdata[1], lastRXdata[2]); if (lastRXdata[1]) { High_A = 0; Low_B.write(0); if (Ldir == 0) wait_us(1000000 / F_MD); High_B = lastRXdata[2] == 0 ? 0 : 1; Low_A.write(1 / 127.0 * lastRXdata[2]); Ldir = 1; } else { High_B = 0; Low_A.write(0); if (Ldir == 1) wait_us(1000000 / F_MD); High_A = 1; Low_B.write(1 - (1 / 127.0 * lastRXdata[2])); Ldir = 0; } break; case 4: break; } } void encoderFollowManager(uint8_t reciveData, uint8_t dataNumber){ } void S_orderFollowManager(uint8_t reciveData, uint8_t dataNumber){ } void com_rx() { LED = !LED; static uint8_t reciveData; reciveData = comPort.getc(); //comPort.printf("%d %f\n", reciveData, timer.read()); //timer.reset(); if (reciveData & LEAD_PACKET_MASK) { nowOrderAddress = reciveData & PACKET_ADDRESS_MASK; //comPort.printf("%d %d\n", nowOrderAddress, address); dataNumber = 0; } //comPort.printf("%d\n", reciveData & PACKET_DATA_MASK); if (nowOrderAddress == address) { dataNumber++; lastRXdata[0] = lastRXdata[1]; lastRXdata[1] = lastRXdata[2]; lastRXdata[2] = reciveData & PACKET_DATA_MASK; if (dataNumber == 1) { mode = (reciveData & PACKET_MODE_MASK) >> 5; } else { switch (mode) { case ORDER_FOLLOWING_MODE: orderFollowManager(reciveData, dataNumber); break; case ENCODER_FOLLOWING_MODE: encoderFollowManager(reciveData, dataNumber); break; case SINGLE_ORDER_MODE: S_orderFollowManager(reciveData, dataNumber); break; } } } } int main() { //comPort.set_flow_control(Serial::RTS, dp17); FP_MD = 1000000 / F_MD; //PWM一周期の周期(マイクロ秒) High_A = 0; High_B = 0; Low_A.period_us(FP_MD); Low_A.write(0); Low_B.period_us(FP_MD); Low_B.write(0); switch5.mode(PullUp); address = ~switch5 & 0b00011111; comPort.printf("%d", address); //初期設定完了 LED = 0; check_reciveData = 0; //High_B = 1; driverEnable = 1; timer.start(); comPort.baud(115200); comPort.format(8, Serial::None, 1); comPort.attach(&com_rx, Serial::RxIrq); while (1) { //comPort.printf("%d\n", address); //Low_A.write((ain.read() - 0.2) * 1.25 < 0 ? 0 : (ain.read() - 0.2) * 1.25); //pc.printf("%f\n", (ain.read() - 0.2) * 1.25 < 0 ? 0 : (ain.read() - 0.2) * 1.25); //comPort.printf("%d\n", address); //comPort.printf("%d\n", address); } }