main.cpp

00001 #include "mbed.h"
00002 
00003 //重要受信データのマスク
00004 #define LEAD_PACKET_MASK        0b10000000
00005 #define PACKET_ADDRESS_MASK     0b00011111
00006 #define PACKET_MODE_MASK        0b01100000
00007 #define PACKET_DATA_MASK        0b01111111
00008 
00009 //モータードライバの動作モード
00010 #define ORDER_FOLLOWING_MODE    1
00011 #define ENCODER_FOLLOWING_MODE  2
00012 #define SINGLE_ORDER_MODE       3 //単命令を受信した後は必ずsendback
00013 
00014 //単命令動作の種類
00015 #define S_ORDER_FREQUENCY           0
00016 #define S_ORDER_SEND_POWER          1
00017 #define S_ORDER_LINEAR_AUTOCHANGE   2
00018 #define S_ORDER_CERTAIN             3
00019 #define S_ORDER_SEND_ENCODER        4
00020 
00021 #define F_MD 15000 //PWMキャリア周波数設定
00022 
00023 /*
00024  * High_A High_B
00025  *   ┃     ┃
00026  *   ┃     ┃
00027  *   ┣━━━┫
00028  *   ┃     ┃
00029  *   ┃     ┃
00030  *  Low_A  Low_B
00031  */
00032  
00033 Serial comPort(dp16, dp15);
00034 DigitalOut driverEnable(dp17);
00035 DigitalOut LED(LED1);
00036 DigitalOut check_reciveData(LED2);
00037 BusIn switch5(dp26, dp25, dp18, dp6, dp4);
00038 //dp5 = ハイサイドPWM1, dp27 = ハイサイドPWM2 （オープンドレイン）
00039 //dp1 = ローサイドPWM1, dp2 = ローサイドPWM2
00040 PwmOut Low_A(dp1);
00041 PwmOut Low_B(dp2);
00042 DigitalOut High_A(dp5);
00043 DigitalOut High_B(dp27);
00044 
00045 Timer timer;
00046 
00047 AnalogIn ain(dp9);
00048 //PortIn encoder(Port0, 0b111);
00049 
00050 
00051 //グローバル変数
00052 uint8_t lastRXdata[3] = {0};
00053 uint8_t address = 0;
00054 uint8_t mode = 0;
00055 uint8_t nowOrderAddress = 0;
00056 uint8_t dataNumber = 0;
00057 uint8_t sendPowerFlag = 0;
00058 uint8_t orderType = 0;
00059 uint8_t Ldir = 1;
00060 int FP_MD;
00061 
00062 void orderFollowManager(uint8_t reciveData, uint8_t dataNumber){
00063     switch (dataNumber) {
00064         case 2:
00065             break;
00066         case 3:
00067             //comPort.printf("%d %d\n", lastRXdata[1], lastRXdata[2]);
00068              if (lastRXdata[1]) {
00069                 High_A = 0;
00070                 Low_B.write(0);
00071                 if (Ldir == 0) wait_us(1000000 / F_MD);
00072                 High_B = lastRXdata[2] == 0 ? 0 : 1;
00073                 Low_A.write(1 / 127.0 * lastRXdata[2]);
00074                 Ldir = 1;
00075             } else {
00076                 High_B = 0;
00077                 Low_A.write(0);
00078                 if (Ldir == 1) wait_us(1000000 / F_MD);
00079                 High_A = 1;
00080                 Low_B.write(1 - (1 / 127.0 * lastRXdata[2]));
00081                 Ldir = 0;
00082             }
00083             break;
00084         case 4:
00085             break;
00086     }
00087 }
00088 
00089 void encoderFollowManager(uint8_t reciveData, uint8_t dataNumber){
00090     
00091 }
00092 
00093 void S_orderFollowManager(uint8_t reciveData, uint8_t dataNumber){
00094     
00095 }
00096 
00097 void com_rx() {
00098     LED = !LED;
00099     static uint8_t reciveData;
00100     reciveData = comPort.getc();
00101     //comPort.printf("%d  %f\n", reciveData, timer.read());
00102     //timer.reset();
00103     
00104     if (reciveData & LEAD_PACKET_MASK) {
00105         nowOrderAddress = reciveData & PACKET_ADDRESS_MASK;
00106         //comPort.printf("%d %d\n", nowOrderAddress, address);
00107         dataNumber = 0;
00108     }
00109     
00110     //comPort.printf("%d\n", reciveData & PACKET_DATA_MASK);
00111     
00112     if (nowOrderAddress == address) {
00113         dataNumber++;
00114         lastRXdata[0] = lastRXdata[1];
00115         lastRXdata[1] = lastRXdata[2];
00116         lastRXdata[2] = reciveData & PACKET_DATA_MASK;
00117         
00118         if (dataNumber == 1) {
00119             mode = (reciveData & PACKET_MODE_MASK) >> 5;
00120         } else {
00121             switch (mode) {
00122                 case ORDER_FOLLOWING_MODE:
00123                     orderFollowManager(reciveData, dataNumber);
00124                     break;
00125                 case ENCODER_FOLLOWING_MODE:
00126                     encoderFollowManager(reciveData, dataNumber);
00127                     break;
00128                 case SINGLE_ORDER_MODE:
00129                     S_orderFollowManager(reciveData, dataNumber);
00130                     break;
00131             }
00132         }    
00133     }
00134 }
00135 
00136 int main() {
00137     //comPort.set_flow_control(Serial::RTS, dp17);
00138 
00139     FP_MD = 1000000 / F_MD; //PWM一周期の周期（マイクロ秒）
00140     
00141     High_A = 0;
00142     High_B = 0;
00143     Low_A.period_us(FP_MD);
00144     Low_A.write(0);
00145     Low_B.period_us(FP_MD);
00146     Low_B.write(0);
00147     
00148     switch5.mode(PullUp);
00149     address = ~switch5 & 0b00011111;
00150     comPort.printf("%d", address);
00151     //初期設定完了
00152     
00153     LED = 0;
00154     check_reciveData = 0;
00155     //High_B = 1;
00156     driverEnable = 1;
00157     
00158     timer.start();
00159     
00160     comPort.baud(115200);
00161     comPort.format(8, Serial::None, 1);
00162     comPort.attach(&com_rx, Serial::RxIrq);
00163     while (1) {
00164         //comPort.printf("%d\n", address);
00165         //Low_A.write((ain.read() - 0.2) * 1.25 < 0 ? 0 : (ain.read() - 0.2) * 1.25);
00166         //pc.printf("%f\n", (ain.read() - 0.2) * 1.25 < 0 ? 0 : (ain.read() - 0.2) * 1.25);
00167         //comPort.printf("%d\n", address);
00168         //comPort.printf("%d\n", address);
00169     }
00170 }