Hiroki Tanaka / MDD_L432KC

MDDのプログラム

Dependencies:   arrc_mbed

main.cpp@0:f65bd64d88b7, 2019-08-26 (annotated)

Committer:
TanakaRobo
Date:
Mon Aug 26 07:39:37 2019 +0000
Revision:
0:f65bd64d88b7
Child:
2:da44cd3b6601
MDD

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TanakaRobo 0:f65bd64d88b7 1 #include "mbed.h"
TanakaRobo 0:f65bd64d88b7 2 #include "library/scrp_slave.hpp"
TanakaRobo 0:f65bd64d88b7 3 #include "library/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 }