好久没来,没法编辑了,在后面给贴吧
该上代码了。
小车部分
- #include <IRremote.h>
- #include <LiquidCrystal_I2C.h>
- #include <Wire.h>
- #include <Servo.h>
- Servo myservo;//定义舵机变量名
- LiquidCrystal_I2C lcd(0x27, 16, 2); // set the LCD address to 0x27 for a 16 chars and 2 line display
- unsigned long lastMillis = millis();
- const int LightPin = 0;
- const int TemPin = 3;
- const int L298nIn1 = 2;
- const int L298nIn2 = 3;
- const int L298nIn3 = 4;
- const int L298nIn4 = 5;
- const int EchoPinR = 6;
- const int TrigPinR = 7;
- const int TrigPinM = 8;
- const int EchoPinM = 9;
- const int EchoPinL = 11;
- const int TrigPinL = 12;
- const int ServoPin = 10;
- const int IRPin = 11;
- const long on0 = 0x00FF6897;
- const long on1 = 0x00FF30CF;
- const long on2 = 0x00FF18E7;
- const long on3 = 0x00FF7A85;
- const long on4 = 0x00FF10EF;
- const long on5 = 0x00FF38C7;
- const long on6 = 0x00FF5AA5;
- const long on7 = 0x00FF42BD;
- const long on8 = 0x00FF4AB5;
- const long on9 = 0x00FF52AD;
- const int KeepAway = 25;
- const int KeepAwayS = 10;
- const int LookDelay = 300;
- const int TurnDelay = 500;
- const int BackDelay = 600;
- const int ReboundDelay = 150;
- String Mode = "M";
- IRrecv irrecv(IRPin);
- decode_results results;
- void setup()
- {
- Serial.begin(115200);
- myservo.attach(10);//定义舵机接口(9、10 都可以,缺点只能控制2 个)
- lcd.init();
- lcd.backlight();
- pinMode(EchoPinM, INPUT);
- pinMode(TrigPinM, OUTPUT);
- pinMode(EchoPinL, INPUT);
- pinMode(TrigPinL, OUTPUT);
- pinMode(EchoPinR, INPUT);
- pinMode(TrigPinR, OUTPUT);
- pinMode(IRPin, INPUT);
- pinMode(L298nIn1, OUTPUT);
- pinMode(L298nIn2, OUTPUT);
- pinMode(L298nIn3, OUTPUT);
- pinMode(L298nIn4, OUTPUT);
- irrecv.enableIRIn(); // Start the receiver
- lcd.print("I am Dogeva!");
- }
- int readTem() {
- int val;//定义变量
- int dat;//定义变量
- val = analogRead(TemPin); // 读取传感器的模拟值并赋值给val
- dat = map(val, 0, 1023, 0, 100);
- //dat = (125 * val) >> 8; //温度计算公式
- return dat;
- }
- int readLight() {
- int val;
- val = analogRead(LightPin);
- val = map(val, 0, 1023, 0, 1000);
- return 1000 - val;
- }
- void goForward() {
- digitalWrite(L298nIn1, HIGH);
- digitalWrite(L298nIn2, LOW);
- digitalWrite(L298nIn3, LOW);
- digitalWrite(L298nIn4, HIGH);
- }
- void goBack() {
- digitalWrite(L298nIn1, LOW);
- digitalWrite(L298nIn2, HIGH);
- digitalWrite(L298nIn3, HIGH);
- digitalWrite(L298nIn4, LOW);
- }
- void goStop() {
- digitalWrite(L298nIn1, LOW);
- digitalWrite(L298nIn2, LOW);
- digitalWrite(L298nIn3, LOW);
- digitalWrite(L298nIn4, LOW);
- }
- void goRebound() {
- digitalWrite(L298nIn1, LOW);
- digitalWrite(L298nIn2, HIGH);
- digitalWrite(L298nIn3, HIGH);
- digitalWrite(L298nIn4, LOW);
- delay(ReboundDelay);
- goStop();
- }
- void turnRight1() {
- digitalWrite(L298nIn1, LOW);
- digitalWrite(L298nIn2, HIGH);
- digitalWrite(L298nIn3, LOW);
- digitalWrite(L298nIn4, HIGH);
-
- }
- void turnRight2() {
- digitalWrite(L298nIn1, LOW);
- digitalWrite(L298nIn2, HIGH);
- digitalWrite(L298nIn3, LOW);
- digitalWrite(L298nIn4, HIGH);
- }
- void turnLeft4() {
- digitalWrite(L298nIn1, HIGH);
- digitalWrite(L298nIn2, LOW);
- digitalWrite(L298nIn3, HIGH);
- digitalWrite(L298nIn4, LOW);
- }
- void turnLeft5() {
- digitalWrite(L298nIn1, HIGH);
- digitalWrite(L298nIn2, LOW);
- digitalWrite(L298nIn3, HIGH);
- digitalWrite(L298nIn4, LOW);
- }
- int IRNumber() {
- if (irrecv.decode(&results))
- {
- // If it's been at least 1/4 second since the last
- // IR received, toggle the relay
- if (millis() - lastMillis > 250)
- {
- //on = !on;
- //digitalWrite(13, on ? HIGH : LOW);
- IRDump(&results);
- }
- lastMillis = millis();
- irrecv.resume(); // Receive the next value
- switch (results.value) {
- case on0:
- return 0;
- case on1:
- return 1;
- case on2:
- return 2;
- case on3:
- return 3;
- case on4:
- return 4;
- case on5:
- return 5;
- case on6:
- return 6;
- case on7:
- return 7;
- case on8:
- return 8;
- case on9:
- return 9;
- default:
- return -1;
- }
- }
- }
- void IRDump(decode_results *results) {
- int count = results->rawlen;
- if (results->decode_type == UNKNOWN)
- {
- Serial.println("Could not decode message");
- }
- else
- {
- if (results->decode_type == NEC)
- {
- Serial.print("Decoded NEC: ");
- }
- else if (results->decode_type == SONY)
- {
- Serial.print("Decoded SONY: ");
- }
- else if (results->decode_type == RC5)
- {
- Serial.print("Decoded RC5: ");
- }
- else if (results->decode_type == RC6)
- {
- Serial.print("Decoded RC6: ");
- }
- Serial.print(results->value, HEX);
- Serial.print(" (");
- Serial.print(results->bits, DEC);
- Serial.println(" bits)");
- }
- Serial.print("Raw (");
- Serial.print(count, DEC);
- Serial.print("): ");
-
- for (int i = 0; i < count; i++)
- {
- if ((i % 2) == 1) {
- Serial.print(results->rawbuf[i]*USECPERTICK, DEC);
- }
- else
- {
- Serial.print(-(int)results->rawbuf[i]*USECPERTICK, DEC);
- }
- Serial.print(" ");
- }
- Serial.println("");
- }
- float checkDistance(int TrigPin, int EchoPin) {
- float distance;
- digitalWrite(TrigPin, LOW);
- delayMicroseconds(2);
- digitalWrite(TrigPin, HIGH);
- delayMicroseconds(10);
- digitalWrite(TrigPin, LOW);
- // 检测脉冲宽度,并计算出距离
- distance = pulseIn(EchoPin, HIGH) / 58.00;
- return distance;
- }
- int lookAround() {
- lookRight1();
- int D1 = checkDistance(TrigPinM, EchoPinM);
- lookRight2();
- int D2 = checkDistance(TrigPinM, EchoPinM);
- lookForward();
- int D3 = checkDistance(TrigPinM, EchoPinM);
- lookLeft4();
- int D4 = checkDistance(TrigPinM, EchoPinM);
- lookLeft5();
- int D5 = checkDistance(TrigPinM, EchoPinM);
- if (D1 > D2 && D1 > D3 && D1 > D4 && D1 > D5 && D1 > KeepAway)return 1;
- if (D2 > D1 && D2 > D3 && D2 > D4 && D2 > D5 && D2 > KeepAway)return 2;
- if (D3 > D2 && D3 > D1 && D3 > D4 && D3 > D5 && D3 > KeepAway)return 3;
- if (D4 > D2 && D4 > D3 && D4 > D1 && D4 > D5 && D4 > KeepAway)return 4;
- if (D5 > D2 && D5 > D3 && D5 > D4 && D5 > D1 && D5 > KeepAway)return 5;
- return 0;
- }
- void lookRight1() {
- myservo.write(5);
- delay(LookDelay);
- }
- void lookRight2() {
- myservo.write(45);
- delay(LookDelay);
- }
- void lookForward() {
- myservo.write(90);
- delay(LookDelay);
- }
- void lookLeft4() {
- myservo.write(135);
- delay(LookDelay);
- }
- void lookLeft5() {
- myservo.write(175);
- delay(LookDelay);
- }
- void autoMove() {
- lcd.clear();
- lcd.setCursor(0, 1) ; //设置光标位置为第二行第一个位置
- lookForward();
- int distanceM = checkDistance(TrigPinM, EchoPinM);
- int distanceR = checkDistance(TrigPinR, EchoPinR);
- int distanceL = checkDistance(TrigPinL, EchoPinL);
- if (distanceM > KeepAway && distanceR > KeepAwayS && distanceL > KeepAwayS) {
- goForward();
- } else {
- for (int i = 0; i < 100; i++)
- {
- goStop();
- goRebound();
- if (distanceR <= KeepAwayS) {
- turnLeft4();
- return;
- }
- if (distanceL <= KeepAwayS) {
- turnRight2();
- return;
- }
- switch (lookAround()) {
- case 1:
- turnRight1();
- delay(TurnDelay);
- goStop();
- return;
- case 2:
- turnRight2();
- delay(TurnDelay / 2);
- goStop();
- return;
- case 3:
- goForward();
- return;
- case 4:
- turnLeft4();
- delay(TurnDelay / 2);
- goStop();
- return;
- case 5:
- turnLeft5();
- delay(TurnDelay);
- goStop();
- return;
- default:
- break;
- }
- goBack();
- delay(BackDelay);
- goStop();
- }
- }
- }
- void manualMove(String s) {
- if (s.length() > 0) {
- lcd.clear();
- lcd.setCursor(0, 1) ; //设置光标位置为第二行第一个位置
- if (s.length() == 1) {
- if (String(s) == String("0")) {
- lcd.print("Stop");
- goStop();
- }
- if (String(s) == String("1")) {
- lcd.print("Go Forward");
- goForward();
- }
- if (String(s) == String("2")) {
- lcd.print("Turn Left");
- turnLeft5();
- }
- if (String(s) == String("3")) {
- lcd.print("Go Back");
- goBack();
- }
- if (String(s) == String("4")) {
- lcd.print("Turn Right");
- turnRight1();
- }
- }
- if (s.length() > 1) {
- lcd.print(s);
- }
- }
- }
- void loop() {
- String comdata = "";
- while (Serial.available() > 0) {
- comdata += char(Serial.read());
- delay(2);
- }
- if (comdata == "A" && Mode != "A") {
- lcd.clear();
- lcd.setCursor(0, 0) ; //设置光标位置为第二行第一个位置
- lcd.print("Auto Mode");
- goStop();
- lookForward();
- Mode = "A";
- }
- if (comdata == "M" && Mode != "M") {
- lcd.clear();
- lcd.setCursor(0, 0) ; //设置光标位置为第二行第一个位置
- lcd.print("Manual Mode");
- goStop();
- lookForward();
- Mode = "M";
- }
- if (Mode == "A") autoMove();
- if (Mode == "M") manualMove(comdata);
- /*lcd.clear();
- lcd.setCursor(0, 1) ; //设置光标位置为第二行第一个位置
- lcd.print("Tep:");
- lcd.print(readTem());
- Serial.println(readTem());
- lcd.print("C Lt:");
- lcd.print(readLight());
- Serial.println(readLight());
- delay(1000);
- String comdata = "";
- while (Serial.available() > 0) {
- comdata += char(Serial.read());
- delay(2);
- }
- if (comdata.length() > 0)
- {
- //lcd.clear();
- if (comdata.length() == 1) {
- if (String(comdata) == String("0")) {
- lcd.print("Stop");
- goStop();
- }
- if (String(comdata) == String("1")) {
- lcd.print("Go Forward");
- goForward();
- }
- if (String(comdata) == String("2")) {
- lcd.print("Turn Left");
- turnLeft5();
- }
- if (String(comdata) == String("3")) {
- lcd.print("Go Back");
- goBack();
- }
- if (String(comdata) == String("4")) {
- lcd.print("Turn Right");
- turnRight1();
- }
- }
- if (comdata.length() > 1) {
- lcd.print(comdata);
- }
- if (comdata.length() > 16) {
- for (int positionCounter = 0; positionCounter < comdata.length() - 16; positionCounter++) {
- // scroll one position left:
- lcd.scrollDisplayLeft();
- // wait a bit:
- delay(600);
- }
- }
- comdata = "";
- }*/
- }
复制代码
路由器部分
lua 脚本
- #!/usr/bin/lua
- io.output("/dev/ttyATH0")
- io.write(os.getenv("QUERY_STRING"))
复制代码
web网页部分
- <html>
- <head>
- </head>
- <body>
- <div style="float:left">
- <script>
- function sendSer(value){
- document.getElementById("ser").src="http://192.168.1.101/cgi-bin/web2ser?"+value;
- }
- </script>
- <table>
- <tr><td/><img id="ser" width="1" height="1">
- <td><input type="button" value="前进" /></td><td/></tr>
- <tr><td><input type="button" value="左转" /></td><td/>
- <td><input type="button" value="右转" /></td></tr>
- <tr><td/><td><input type="button" value="后退" /></td><td/></tr>
- <tr><td colspan="3" align="middle"> <input type="button" value="自动控制" /></td></tr>
- <tr><td colspan="3" align="middle"> <input type="button" value="手动控制" /></td></tr>
- </table>
- </div>
- </body>
- </html>
复制代码
|