SES / Mbed 2 deprecated mbed_blinky

新型モータードライバのメインプログラム

Dependencies:   mbed

main.cpp@4:614e752487cc, 2016-06-24 (annotated)

Committer:
SES01
Date:
Fri Jun 24 11:38:21 2016 +0000
Revision:
4:614e752487cc
Parent:
3:65eaf88bf278 
???????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
SES01 0:cf3829614495 1 #include "mbed.h"
SES01 0:cf3829614495 2
SES01 1:e45830806a1c 3 //重要受信データのマスク
SES01 1:e45830806a1c 4 #define LEAD_PACKET_MASK 0b10000000
SES01 1:e45830806a1c 5 #define PACKET_ADDRESS_MASK 0b00011111
SES01 1:e45830806a1c 6 #define PACKET_MODE_MASK 0b01100000
SES01 4:614e752487cc 7 #define PACKET_DATA_MASK 0b01111111
SES01 1:e45830806a1c 8
SES01 1:e45830806a1c 9 //モータードライバの動作モード
SES01 4:614e752487cc 10 #define ORDER_FOLLOWING_MODE 1
SES01 4:614e752487cc 11 #define ENCODER_FOLLOWING_MODE 2
SES01 4:614e752487cc 12 #define SINGLE_ORDER_MODE 3 //単命令を受信した後は必ずsendback
SES01 2:c77482250d0b 13
SES01 2:c77482250d0b 14 //単命令動作の種類
SES01 2:c77482250d0b 15 #define S_ORDER_FREQUENCY 0
SES01 2:c77482250d0b 16 #define S_ORDER_SEND_POWER 1
SES01 2:c77482250d0b 17 #define S_ORDER_LINEAR_AUTOCHANGE 2
SES01 2:c77482250d0b 18 #define S_ORDER_CERTAIN 3
SES01 2:c77482250d0b 19 #define S_ORDER_SEND_ENCODER 4
SES01 1:e45830806a1c 20
SES01 4:614e752487cc 21 #define F_MD 15000 //PWMキャリア周波数設定
SES01 0:cf3829614495 22
SES01 0:cf3829614495 23 /*
SES01 0:cf3829614495 24 * High_A High_B
SES01 0:cf3829614495 25 * ┃ ┃
SES01 0:cf3829614495 26 * ┃ ┃
SES01 0:cf3829614495 27 * ┣━━━┫
SES01 0:cf3829614495 28 * ┃ ┃
SES01 0:cf3829614495 29 * ┃ ┃
SES01 0:cf3829614495 30 * Low_A Low_B
SES01 0:cf3829614495 31 */
SES01 0:cf3829614495 32
SES01 0:cf3829614495 33 Serial comPort(dp16, dp15);
SES01 4:614e752487cc 34 DigitalOut driverEnable(dp17);
SES01 4:614e752487cc 35 DigitalOut LED(LED1);
SES01 4:614e752487cc 36 DigitalOut check_reciveData(LED2);
SES01 4:614e752487cc 37 BusIn switch5(dp26, dp25, dp18, dp6, dp4);
SES01 1:e45830806a1c 38 //dp5 = ハイサイドPWM1, dp27 = ハイサイドPWM2 （オープンドレイン）
SES01 0:cf3829614495 39 //dp1 = ローサイドPWM1, dp2 = ローサイドPWM2
SES01 0:cf3829614495 40 PwmOut Low_A(dp1);
SES01 0:cf3829614495 41 PwmOut Low_B(dp2);
SES01 0:cf3829614495 42 DigitalOut High_A(dp5);
SES01 0:cf3829614495 43 DigitalOut High_B(dp27);
SES01 0:cf3829614495 44
SES01 4:614e752487cc 45 Timer timer;
SES01 4:614e752487cc 46
SES01 3:65eaf88bf278 47 AnalogIn ain(dp9);
SES01 3:65eaf88bf278 48 //PortIn encoder(Port0, 0b111);
SES01 3:65eaf88bf278 49
SES01 1:e45830806a1c 50
SES01 1:e45830806a1c 51 //グローバル変数
SES01 4:614e752487cc 52 uint8_t lastRXdata[3] = {0};
SES01 1:e45830806a1c 53 uint8_t address = 0;
SES01 1:e45830806a1c 54 uint8_t mode = 0;
SES01 1:e45830806a1c 55 uint8_t nowOrderAddress = 0;
SES01 1:e45830806a1c 56 uint8_t dataNumber = 0;
SES01 1:e45830806a1c 57 uint8_t sendPowerFlag = 0;
SES01 4:614e752487cc 58 uint8_t orderType = 0;
SES01 4:614e752487cc 59 uint8_t Ldir = 1;
SES01 4:614e752487cc 60 int FP_MD;
SES01 1:e45830806a1c 61
SES01 1:e45830806a1c 62 void orderFollowManager(uint8_t reciveData, uint8_t dataNumber){
SES01 2:c77482250d0b 63 switch (dataNumber) {
SES01 2:c77482250d0b 64 case 2:
SES01 2:c77482250d0b 65 break;
SES01 2:c77482250d0b 66 case 3:
SES01 4:614e752487cc 67 //comPort.printf("%d %d\n", lastRXdata[1], lastRXdata[2]);
SES01 4:614e752487cc 68 if (lastRXdata[1]) {
SES01 4:614e752487cc 69 High_A = 0;
SES01 4:614e752487cc 70 Low_B.write(0);
SES01 4:614e752487cc 71 if (Ldir == 0) wait_us(1000000 / F_MD);
SES01 4:614e752487cc 72 High_B = lastRXdata[2] == 0 ? 0 : 1;
SES01 4:614e752487cc 73 Low_A.write(1 / 127.0 * lastRXdata[2]);
SES01 4:614e752487cc 74 Ldir = 1;
SES01 4:614e752487cc 75 } else {
SES01 4:614e752487cc 76 High_B = 0;
SES01 4:614e752487cc 77 Low_A.write(0);
SES01 4:614e752487cc 78 if (Ldir == 1) wait_us(1000000 / F_MD);
SES01 4:614e752487cc 79 High_A = 1;
SES01 4:614e752487cc 80 Low_B.write(1 - (1 / 127.0 * lastRXdata[2]));
SES01 4:614e752487cc 81 Ldir = 0;
SES01 4:614e752487cc 82 }
SES01 2:c77482250d0b 83 break;
SES01 2:c77482250d0b 84 case 4:
SES01 2:c77482250d0b 85 break;
SES01 2:c77482250d0b 86 }
SES01 1:e45830806a1c 87 }
SES01 1:e45830806a1c 88
SES01 1:e45830806a1c 89 void encoderFollowManager(uint8_t reciveData, uint8_t dataNumber){
SES01 1:e45830806a1c 90
SES01 1:e45830806a1c 91 }
SES01 1:e45830806a1c 92
SES01 1:e45830806a1c 93 void S_orderFollowManager(uint8_t reciveData, uint8_t dataNumber){
SES01 1:e45830806a1c 94
SES01 1:e45830806a1c 95 }
SES01 1:e45830806a1c 96
SES01 0:cf3829614495 97 void com_rx() {
SES01 4:614e752487cc 98 LED = !LED;
SES01 1:e45830806a1c 99 static uint8_t reciveData;
SES01 1:e45830806a1c 100 reciveData = comPort.getc();
SES01 4:614e752487cc 101 //comPort.printf("%d %f\n", reciveData, timer.read());
SES01 4:614e752487cc 102 //timer.reset();
SES01 1:e45830806a1c 103
SES01 4:614e752487cc 104 if (reciveData & LEAD_PACKET_MASK) {
SES01 4:614e752487cc 105 nowOrderAddress = reciveData & PACKET_ADDRESS_MASK;
SES01 4:614e752487cc 106 //comPort.printf("%d %d\n", nowOrderAddress, address);
SES01 1:e45830806a1c 107 dataNumber = 0;
SES01 1:e45830806a1c 108 }
SES01 1:e45830806a1c 109
SES01 4:614e752487cc 110 //comPort.printf("%d\n", reciveData & PACKET_DATA_MASK);
SES01 4:614e752487cc 111
SES01 1:e45830806a1c 112 if (nowOrderAddress == address) {
SES01 1:e45830806a1c 113 dataNumber++;
SES01 4:614e752487cc 114 lastRXdata[0] = lastRXdata[1];
SES01 4:614e752487cc 115 lastRXdata[1] = lastRXdata[2];
SES01 4:614e752487cc 116 lastRXdata[2] = reciveData & PACKET_DATA_MASK;
SES01 0:cf3829614495 117
SES01 1:e45830806a1c 118 if (dataNumber == 1) {
SES01 4:614e752487cc 119 mode = (reciveData & PACKET_MODE_MASK) >> 5;
SES01 1:e45830806a1c 120 } else {
SES01 1:e45830806a1c 121 switch (mode) {
SES01 1:e45830806a1c 122 case ORDER_FOLLOWING_MODE:
SES01 1:e45830806a1c 123 orderFollowManager(reciveData, dataNumber);
SES01 1:e45830806a1c 124 break;
SES01 1:e45830806a1c 125 case ENCODER_FOLLOWING_MODE:
SES01 1:e45830806a1c 126 encoderFollowManager(reciveData, dataNumber);
SES01 1:e45830806a1c 127 break;
SES01 1:e45830806a1c 128 case SINGLE_ORDER_MODE:
SES01 1:e45830806a1c 129 S_orderFollowManager(reciveData, dataNumber);
SES01 1:e45830806a1c 130 break;
SES01 1:e45830806a1c 131 }
SES01 1:e45830806a1c 132 }
SES01 1:e45830806a1c 133 }
SES01 0:cf3829614495 134 }
SES01 0:cf3829614495 135
SES01 0:cf3829614495 136 int main() {
SES01 4:614e752487cc 137 //comPort.set_flow_control(Serial::RTS, dp17);
SES01 0:cf3829614495 138
SES01 4:614e752487cc 139 FP_MD = 1000000 / F_MD; //PWM一周期の周期（マイクロ秒）
SES01 0:cf3829614495 140
SES01 0:cf3829614495 141 High_A = 0;
SES01 1:e45830806a1c 142 High_B = 0;
SES01 0:cf3829614495 143 Low_A.period_us(FP_MD);
SES01 0:cf3829614495 144 Low_A.write(0);
SES01 0:cf3829614495 145 Low_B.period_us(FP_MD);
SES01 0:cf3829614495 146 Low_B.write(0);
SES01 3:65eaf88bf278 147
SES01 3:65eaf88bf278 148 switch5.mode(PullUp);
SES01 4:614e752487cc 149 address = ~switch5 & 0b00011111;
SES01 3:65eaf88bf278 150 comPort.printf("%d", address);
SES01 0:cf3829614495 151 //初期設定完了
SES01 0:cf3829614495 152
SES01 4:614e752487cc 153 LED = 0;
SES01 4:614e752487cc 154 check_reciveData = 0;
SES01 4:614e752487cc 155 //High_B = 1;
SES01 0:cf3829614495 156 driverEnable = 1;
SES01 4:614e752487cc 157
SES01 4:614e752487cc 158 timer.start();
SES01 4:614e752487cc 159
SES01 4:614e752487cc 160 comPort.baud(115200);
SES01 4:614e752487cc 161 comPort.format(8, Serial::None, 1);
SES01 4:614e752487cc 162 comPort.attach(&com_rx, Serial::RxIrq);
SES01 0:cf3829614495 163 while (1) {
SES01 4:614e752487cc 164 //comPort.printf("%d\n", address);
SES01 4:614e752487cc 165 //Low_A.write((ain.read() - 0.2) * 1.25 < 0 ? 0 : (ain.read() - 0.2) * 1.25);
SES01 0:cf3829614495 166 //pc.printf("%f\n", (ain.read() - 0.2) * 1.25 < 0 ? 0 : (ain.read() - 0.2) * 1.25);
SES01 4:614e752487cc 167 //comPort.printf("%d\n", address);
SES01 4:614e752487cc 168 //comPort.printf("%d\n", address);
SES01 0:cf3829614495 169 }
SES01 0:cf3829614495 170 }