MDDのプログラム
main.cpp@2:da44cd3b6601, 2020-07-24 (annotated)
- Committer:
- TanakaRobo
- Date:
- Fri Jul 24 14:14:12 2020 +0000
- Revision:
- 2:da44cd3b6601
- Parent:
- 0:f65bd64d88b7
update serial interrupt
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
TanakaRobo | 0:f65bd64d88b7 | 1 | #include "mbed.h" |
TanakaRobo | 2:da44cd3b6601 | 2 | #include "scrp_slave.hpp" |
TanakaRobo | 2:da44cd3b6601 | 3 | #include "rotary_inc.hpp" |
TanakaRobo | 0:f65bd64d88b7 | 4 | #define MAXPWM 250 |
TanakaRobo | 0:f65bd64d88b7 | 5 | #define Period 256 |
TanakaRobo | 0:f65bd64d88b7 | 6 | |
TanakaRobo | 0:f65bd64d88b7 | 7 | ScrpSlave slave(PA_9,PA_10,PA_12,SERIAL_TX,SERIAL_RX,0x0803e000); |
TanakaRobo | 0:f65bd64d88b7 | 8 | |
TanakaRobo | 0:f65bd64d88b7 | 9 | const PinName pwmpin[5][3] = { |
TanakaRobo | 0:f65bd64d88b7 | 10 | {PB_0 ,PB_1 ,PB_3}, |
TanakaRobo | 0:f65bd64d88b7 | 11 | {PA_1 ,PA_3 ,PB_4}, |
TanakaRobo | 0:f65bd64d88b7 | 12 | {PA_8 ,PA_7 ,PB_5}, |
TanakaRobo | 0:f65bd64d88b7 | 13 | {PB_6 ,PA_11,PB_7}, |
TanakaRobo | 0:f65bd64d88b7 | 14 | {PA_0 ,PA_4 ,NC}//ロータリーエンコーダー用 |
TanakaRobo | 0:f65bd64d88b7 | 15 | }; |
TanakaRobo | 0:f65bd64d88b7 | 16 | |
TanakaRobo | 0:f65bd64d88b7 | 17 | //const PinName rotarypin[2] = {PA_0,PA_4}; |
TanakaRobo | 0:f65bd64d88b7 | 18 | |
TanakaRobo | 0:f65bd64d88b7 | 19 | //RotaryInc rotary(rotarypin[0],rotarypin[1],10,200,0); |
TanakaRobo | 0:f65bd64d88b7 | 20 | uint8_t flag[5]; |
TanakaRobo | 0:f65bd64d88b7 | 21 | |
TanakaRobo | 0:f65bd64d88b7 | 22 | bool Drive(int id,int pwm){ |
TanakaRobo | 0:f65bd64d88b7 | 23 | pwm = constrain(pwm,-MAXPWM,MAXPWM); |
TanakaRobo | 0:f65bd64d88b7 | 24 | DigitalOut Led(pwmpin[id][2]); |
TanakaRobo | 0:f65bd64d88b7 | 25 | if(!pwm){ |
TanakaRobo | 0:f65bd64d88b7 | 26 | DigitalOut Moter1(pwmpin[id][0],0); |
TanakaRobo | 0:f65bd64d88b7 | 27 | DigitalOut Moter2(pwmpin[id][1],0); |
TanakaRobo | 0:f65bd64d88b7 | 28 | flag[id] = 0; |
TanakaRobo | 0:f65bd64d88b7 | 29 | Led.write(0); |
TanakaRobo | 0:f65bd64d88b7 | 30 | }else if(0 < pwm){ |
TanakaRobo | 0:f65bd64d88b7 | 31 | PwmOut Moter1(pwmpin[id][0]); |
TanakaRobo | 0:f65bd64d88b7 | 32 | Moter1.period_us(Period); |
TanakaRobo | 0:f65bd64d88b7 | 33 | Moter1.write((float)pwm/255); |
TanakaRobo | 0:f65bd64d88b7 | 34 | DigitalOut Moter2(pwmpin[id][1],0); |
TanakaRobo | 0:f65bd64d88b7 | 35 | flag[id] = 2; |
TanakaRobo | 0:f65bd64d88b7 | 36 | Led.write(1); |
TanakaRobo | 0:f65bd64d88b7 | 37 | }else{ |
TanakaRobo | 0:f65bd64d88b7 | 38 | DigitalOut Moter1(pwmpin[id][0],0); |
TanakaRobo | 0:f65bd64d88b7 | 39 | PwmOut Moter2(pwmpin[id][1]); |
TanakaRobo | 0:f65bd64d88b7 | 40 | Moter2.period_us(Period); |
TanakaRobo | 0:f65bd64d88b7 | 41 | Moter2.write((float)-pwm/255); |
TanakaRobo | 0:f65bd64d88b7 | 42 | flag[id] = 1; |
TanakaRobo | 0:f65bd64d88b7 | 43 | Led.write(1); |
TanakaRobo | 0:f65bd64d88b7 | 44 | } |
TanakaRobo | 0:f65bd64d88b7 | 45 | return true; |
TanakaRobo | 0:f65bd64d88b7 | 46 | } |
TanakaRobo | 0:f65bd64d88b7 | 47 | |
TanakaRobo | 0:f65bd64d88b7 | 48 | bool solenoid(int id,int data){ |
TanakaRobo | 0:f65bd64d88b7 | 49 | if(data == 0){ |
TanakaRobo | 0:f65bd64d88b7 | 50 | flag[id] = 0; |
TanakaRobo | 0:f65bd64d88b7 | 51 | DigitalOut Moter1(pwmpin[id][0],0); |
TanakaRobo | 0:f65bd64d88b7 | 52 | DigitalOut Moter2(pwmpin[id][1],0); |
TanakaRobo | 0:f65bd64d88b7 | 53 | }else if(data == 1){ |
TanakaRobo | 0:f65bd64d88b7 | 54 | flag[id] |= 0x01; |
TanakaRobo | 0:f65bd64d88b7 | 55 | DigitalOut Moter1(pwmpin[id][0],1); |
TanakaRobo | 0:f65bd64d88b7 | 56 | }else if(data == 2){ |
TanakaRobo | 0:f65bd64d88b7 | 57 | flag[id] |= 0x02; |
TanakaRobo | 0:f65bd64d88b7 | 58 | DigitalOut Moter2(pwmpin[id][1],1); |
TanakaRobo | 0:f65bd64d88b7 | 59 | }else if(data == -1){ |
TanakaRobo | 0:f65bd64d88b7 | 60 | flag[id] &= 0xfe; |
TanakaRobo | 0:f65bd64d88b7 | 61 | DigitalOut Moter1(pwmpin[id][0],0); |
TanakaRobo | 0:f65bd64d88b7 | 62 | }else if(data == -2){ |
TanakaRobo | 0:f65bd64d88b7 | 63 | flag[id] &= 0xfd; |
TanakaRobo | 0:f65bd64d88b7 | 64 | DigitalOut Moter2(pwmpin[id][1],0); |
TanakaRobo | 0:f65bd64d88b7 | 65 | } |
TanakaRobo | 0:f65bd64d88b7 | 66 | if(flag[id] == 0){ |
TanakaRobo | 0:f65bd64d88b7 | 67 | DigitalOut Led(pwmpin[id][2],0); |
TanakaRobo | 0:f65bd64d88b7 | 68 | }else{ |
TanakaRobo | 0:f65bd64d88b7 | 69 | DigitalOut Led(pwmpin[id][2],1); |
TanakaRobo | 0:f65bd64d88b7 | 70 | } |
TanakaRobo | 0:f65bd64d88b7 | 71 | return true; |
TanakaRobo | 0:f65bd64d88b7 | 72 | } |
TanakaRobo | 0:f65bd64d88b7 | 73 | |
TanakaRobo | 0:f65bd64d88b7 | 74 | bool safe(int rx_data,int &tx_data){ |
TanakaRobo | 0:f65bd64d88b7 | 75 | for(int i = 0;i < 5;i++){ |
TanakaRobo | 0:f65bd64d88b7 | 76 | DigitalOut Moter1(pwmpin[i][0],0); |
TanakaRobo | 0:f65bd64d88b7 | 77 | DigitalOut Moter2(pwmpin[i][1],0); |
TanakaRobo | 0:f65bd64d88b7 | 78 | DigitalOut Led(pwmpin[i][2],0); |
TanakaRobo | 0:f65bd64d88b7 | 79 | } |
TanakaRobo | 0:f65bd64d88b7 | 80 | return true; |
TanakaRobo | 0:f65bd64d88b7 | 81 | } |
TanakaRobo | 0:f65bd64d88b7 | 82 | |
TanakaRobo | 0:f65bd64d88b7 | 83 | bool DM1(int rx_data,int &tx_data){ |
TanakaRobo | 0:f65bd64d88b7 | 84 | return Drive(0,rx_data); |
TanakaRobo | 0:f65bd64d88b7 | 85 | } |
TanakaRobo | 0:f65bd64d88b7 | 86 | |
TanakaRobo | 0:f65bd64d88b7 | 87 | bool DM2(int rx_data,int &tx_data){ |
TanakaRobo | 0:f65bd64d88b7 | 88 | return Drive(1,rx_data); |
TanakaRobo | 0:f65bd64d88b7 | 89 | } |
TanakaRobo | 0:f65bd64d88b7 | 90 | |
TanakaRobo | 0:f65bd64d88b7 | 91 | bool DM3(int rx_data,int &tx_data){ |
TanakaRobo | 0:f65bd64d88b7 | 92 | return Drive(2,rx_data); |
TanakaRobo | 0:f65bd64d88b7 | 93 | } |
TanakaRobo | 0:f65bd64d88b7 | 94 | |
TanakaRobo | 0:f65bd64d88b7 | 95 | bool DM4(int rx_data,int &tx_data){ |
TanakaRobo | 0:f65bd64d88b7 | 96 | return Drive(3,rx_data); |
TanakaRobo | 0:f65bd64d88b7 | 97 | } |
TanakaRobo | 0:f65bd64d88b7 | 98 | |
TanakaRobo | 0:f65bd64d88b7 | 99 | bool SL1(int rx_data,int &tx_data){ |
TanakaRobo | 0:f65bd64d88b7 | 100 | return solenoid(0,rx_data); |
TanakaRobo | 0:f65bd64d88b7 | 101 | } |
TanakaRobo | 0:f65bd64d88b7 | 102 | |
TanakaRobo | 0:f65bd64d88b7 | 103 | bool SL2(int rx_data,int &tx_data){ |
TanakaRobo | 0:f65bd64d88b7 | 104 | return solenoid(1,rx_data); |
TanakaRobo | 0:f65bd64d88b7 | 105 | } |
TanakaRobo | 0:f65bd64d88b7 | 106 | |
TanakaRobo | 0:f65bd64d88b7 | 107 | bool SL3(int rx_data,int &tx_data){ |
TanakaRobo | 0:f65bd64d88b7 | 108 | return solenoid(2,rx_data); |
TanakaRobo | 0:f65bd64d88b7 | 109 | } |
TanakaRobo | 0:f65bd64d88b7 | 110 | |
TanakaRobo | 0:f65bd64d88b7 | 111 | bool SL4(int rx_data,int &tx_data){ |
TanakaRobo | 0:f65bd64d88b7 | 112 | return solenoid(3,rx_data); |
TanakaRobo | 0:f65bd64d88b7 | 113 | } |
TanakaRobo | 0:f65bd64d88b7 | 114 | |
TanakaRobo | 0:f65bd64d88b7 | 115 | bool RO(int rx_data,int &tx_data){ |
TanakaRobo | 0:f65bd64d88b7 | 116 | //tx_data = rotary.get(); |
TanakaRobo | 0:f65bd64d88b7 | 117 | //return true; |
TanakaRobo | 0:f65bd64d88b7 | 118 | return solenoid(4,rx_data); |
TanakaRobo | 0:f65bd64d88b7 | 119 | } |
TanakaRobo | 0:f65bd64d88b7 | 120 | |
TanakaRobo | 0:f65bd64d88b7 | 121 | int main(){ |
TanakaRobo | 0:f65bd64d88b7 | 122 | Drive(0,0); |
TanakaRobo | 0:f65bd64d88b7 | 123 | Drive(1,0); |
TanakaRobo | 0:f65bd64d88b7 | 124 | Drive(2,0); |
TanakaRobo | 0:f65bd64d88b7 | 125 | Drive(3,0); |
TanakaRobo | 0:f65bd64d88b7 | 126 | solenoid(4,0); |
TanakaRobo | 0:f65bd64d88b7 | 127 | slave.addCMD(2,DM1); |
TanakaRobo | 0:f65bd64d88b7 | 128 | slave.addCMD(3,DM2); |
TanakaRobo | 0:f65bd64d88b7 | 129 | slave.addCMD(4,DM3); |
TanakaRobo | 0:f65bd64d88b7 | 130 | slave.addCMD(5,DM4); |
TanakaRobo | 0:f65bd64d88b7 | 131 | slave.addCMD(6,SL1); |
TanakaRobo | 0:f65bd64d88b7 | 132 | slave.addCMD(7,SL2); |
TanakaRobo | 0:f65bd64d88b7 | 133 | slave.addCMD(8,SL3); |
TanakaRobo | 0:f65bd64d88b7 | 134 | slave.addCMD(9,SL4); |
TanakaRobo | 0:f65bd64d88b7 | 135 | slave.addCMD(10,RO); |
TanakaRobo | 0:f65bd64d88b7 | 136 | slave.addCMD(255,safe); |
TanakaRobo | 0:f65bd64d88b7 | 137 | while(true); |
TanakaRobo | 0:f65bd64d88b7 | 138 | } |