MDDのプログラム
Diff: main.cpp
- Revision:
- 0:f65bd64d88b7
- Child:
- 2:da44cd3b6601
diff -r 000000000000 -r f65bd64d88b7 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Aug 26 07:39:37 2019 +0000 @@ -0,0 +1,138 @@ +#include "mbed.h" +#include "library/scrp_slave.hpp" +#include "library/rotary_inc.hpp" +#define MAXPWM 250 +#define Period 256 + +ScrpSlave slave(PA_9,PA_10,PA_12,SERIAL_TX,SERIAL_RX,0x0803e000); + +const PinName pwmpin[5][3] = { + {PB_0 ,PB_1 ,PB_3}, + {PA_1 ,PA_3 ,PB_4}, + {PA_8 ,PA_7 ,PB_5}, + {PB_6 ,PA_11,PB_7}, + {PA_0 ,PA_4 ,NC}//ロータリーエンコーダー用 +}; + +//const PinName rotarypin[2] = {PA_0,PA_4}; + +//RotaryInc rotary(rotarypin[0],rotarypin[1],10,200,0); +uint8_t flag[5]; + +bool Drive(int id,int pwm){ + pwm = constrain(pwm,-MAXPWM,MAXPWM); + DigitalOut Led(pwmpin[id][2]); + if(!pwm){ + DigitalOut Moter1(pwmpin[id][0],0); + DigitalOut Moter2(pwmpin[id][1],0); + flag[id] = 0; + Led.write(0); + }else if(0 < pwm){ + PwmOut Moter1(pwmpin[id][0]); + Moter1.period_us(Period); + Moter1.write((float)pwm/255); + DigitalOut Moter2(pwmpin[id][1],0); + flag[id] = 2; + Led.write(1); + }else{ + DigitalOut Moter1(pwmpin[id][0],0); + PwmOut Moter2(pwmpin[id][1]); + Moter2.period_us(Period); + Moter2.write((float)-pwm/255); + flag[id] = 1; + Led.write(1); + } + return true; +} + +bool solenoid(int id,int data){ + if(data == 0){ + flag[id] = 0; + DigitalOut Moter1(pwmpin[id][0],0); + DigitalOut Moter2(pwmpin[id][1],0); + }else if(data == 1){ + flag[id] |= 0x01; + DigitalOut Moter1(pwmpin[id][0],1); + }else if(data == 2){ + flag[id] |= 0x02; + DigitalOut Moter2(pwmpin[id][1],1); + }else if(data == -1){ + flag[id] &= 0xfe; + DigitalOut Moter1(pwmpin[id][0],0); + }else if(data == -2){ + flag[id] &= 0xfd; + DigitalOut Moter2(pwmpin[id][1],0); + } + if(flag[id] == 0){ + DigitalOut Led(pwmpin[id][2],0); + }else{ + DigitalOut Led(pwmpin[id][2],1); + } + return true; +} + +bool safe(int rx_data,int &tx_data){ + for(int i = 0;i < 5;i++){ + DigitalOut Moter1(pwmpin[i][0],0); + DigitalOut Moter2(pwmpin[i][1],0); + DigitalOut Led(pwmpin[i][2],0); + } + return true; +} + +bool DM1(int rx_data,int &tx_data){ + return Drive(0,rx_data); +} + +bool DM2(int rx_data,int &tx_data){ + return Drive(1,rx_data); +} + +bool DM3(int rx_data,int &tx_data){ + return Drive(2,rx_data); +} + +bool DM4(int rx_data,int &tx_data){ + return Drive(3,rx_data); +} + +bool SL1(int rx_data,int &tx_data){ + return solenoid(0,rx_data); +} + +bool SL2(int rx_data,int &tx_data){ + return solenoid(1,rx_data); +} + +bool SL3(int rx_data,int &tx_data){ + return solenoid(2,rx_data); +} + +bool SL4(int rx_data,int &tx_data){ + return solenoid(3,rx_data); +} + +bool RO(int rx_data,int &tx_data){ + //tx_data = rotary.get(); + //return true; + return solenoid(4,rx_data); +} + +int main(){ + Drive(0,0); + Drive(1,0); + Drive(2,0); + Drive(3,0); + solenoid(4,0); + slave.addCMD(2,DM1); + slave.addCMD(3,DM2); + slave.addCMD(4,DM3); + slave.addCMD(5,DM4); + slave.addCMD(6,SL1); + slave.addCMD(7,SL2); + slave.addCMD(8,SL3); + slave.addCMD(9,SL4); + slave.addCMD(10,RO); + slave.addCMD(255,safe); + while(true); +}