1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218
| #include <SR04.h> #include <Wire.h> #include <LiquidCrystal_I2C.h>
#define EM_Pulse_common 7 #define EM_1_3_dirY_common 3 PIN脚。电机1为左方上下移动电机,电机3为右方上下移动电机 #define EM_2_4_dirX_common 5 #define EM_enable_M1 4 #define EM_enable_M2 6 #define EM_enable_M3 13 #define EM_enable_M4 12
#define B_General 2
#define B_U 11 #define B_D 10 #define B_L 9 #define B_R 8
int State_B_U = 0; int State_B_D = 0; int State_B_L = 0; int State_B_R = 0; bool Choice_LorR = 0;
const int UV1_TrigPin = A1; const int UV1_EchoPin = A2; const int UV2_TrigPin = A3; const int UV2_EchoPin = A4; const int UV3_TrigPin = A5; const int UV3_EchoPin = A6; const int UV4_TrigPin = A7; const int UV4_EchoPin = A8;
SR04 ultrasonic1 = SR04(UV1_TrigPin, UV1_EchoPin); SR04 ultrasonic2 = SR04(UV2_TrigPin, UV2_EchoPin); SR04 ultrasonic3 = SR04(UV3_TrigPin, UV3_EchoPin); SR04 ultrasonic4 = SR04(UV4_TrigPin, UV4_EchoPin);
LiquidCrystal_I2C lcd(0x27, 20, 4);
void setup() { Serial.begin(9600);
pinMode(B_General, INPUT); pinMode(B_U, INPUT); pinMode(B_D, INPUT); pinMode(B_L, INPUT); pinMode(B_R, INPUT);
pinMode(EM_1_3_dirY_common, OUTPUT); pinMode(EM_2_4_dirX_common, OUTPUT); pinMode(EM_enable_M1, OUTPUT); pinMode(EM_enable_M2, OUTPUT); pinMode(EM_enable_M3, OUTPUT); pinMode(EM_enable_M4, OUTPUT); pinMode(EM_Pulse_common, OUTPUT);
digitalWrite(EM_enable_M1, LOW); digitalWrite(EM_enable_M2, LOW); digitalWrite(EM_enable_M3, LOW); digitalWrite(EM_enable_M4, LOW); lcd.init(); lcd.backlight(); }
void loop() { tone(EM_Pulse_common, 8000);
if (digitalRead(B_General) == HIGH) { Serial.println("in loop"); Choice_LorR = !(Choice_LorR); Serial.println(Choice_LorR); delay(500); }
State_B_U = digitalRead(B_U); State_B_D = digitalRead(B_D); State_B_L = digitalRead(B_L); State_B_R = digitalRead(B_R);
if (Choice_LorR == 0) { if (State_B_U == HIGH) { Serial.println("Left UP"); digitalWrite(EM_enable_M1, HIGH); Serial.println("after enable"); digitalWrite(EM_1_3_dirY_common, HIGH); delay(50); }
else if (State_B_D == HIGH) { Serial.println("Left down"); digitalWrite(EM_enable_M1, HIGH); digitalWrite(EM_1_3_dirY_common, LOW); delay(50); }
else if (State_B_L == HIGH) { Serial.println("Left left"); digitalWrite(EM_enable_M2, HIGH); digitalWrite(EM_2_4_dirX_common, HIGH); delay(50); }
else if (State_B_R == HIGH) { Serial.println("Left right"); digitalWrite(EM_enable_M2, HIGH); digitalWrite(EM_2_4_dirX_common, LOW); delay(50); } } else { if (State_B_U == HIGH) { Serial.println("Right UP"); tone(EM_Pulse_common, 8000); digitalWrite(EM_enable_M3, HIGH); digitalWrite(EM_1_3_dirY_common, HIGH); delay(50); }
else if (State_B_D == HIGH) { Serial.println("Right down"); tone(EM_Pulse_common, 8000); digitalWrite(EM_enable_M3, HIGH); digitalWrite(EM_1_3_dirY_common, LOW); delay(50); }
else if (State_B_L == HIGH) { Serial.println("Right left"); digitalWrite(EM_enable_M4, HIGH); digitalWrite(EM_2_4_dirX_common, LOW); delay(50); } else if (State_B_R == HIGH) { Serial.println("Right right"); digitalWrite(EM_enable_M4, HIGH); digitalWrite(EM_2_4_dirX_common, HIGH); delay(50); } } digitalWrite(EM_enable_M1, LOW); digitalWrite(EM_enable_M2, LOW); digitalWrite(EM_enable_M3, LOW); digitalWrite(EM_enable_M4, LOW); float distance_2_Left_X = 440-ultrasonic2.GetDistance() * 10; Serial.println(distance_2_Left_X); lcd.setCursor(0, 0); lcd.print("Left(mm)"); lcd.setCursor(11, 0); lcd.print("Right(mm)"); lcd.setCursor(0, 1); lcd.print("X="); lcd.print(distance_2_Left_X, 1); float distance_4_Right_X = 450-ultrasonic4.GetDistance()*10; Serial.println(distance_4_Right_X); lcd.setCursor(11, 1); lcd.print("X="); lcd.print(distance_4_Right_X,1); float distance_1_Left_Y = 65+ultrasonic1.GetDistance()*10; Serial.println(distance_1_Left_Y); lcd.setCursor(0, 2); lcd.print("Y="); lcd.print(distance_1_Left_Y,1); float distance_3_Right_Y = 65+ultrasonic3.GetDistance()*10; Serial.println(distance_3_Right_Y); lcd.setCursor(11, 2); lcd.print("Y="); lcd.print(distance_3_Right_Y,1); }
|