Аннотация

программа control_motor управляет моторами а также web камерой подвижной платформы в зависимости от принятых управляющих сигналов с raspberry.

#include <Servo.h> #include "Wire.h" #include "I2Cdev.h" #include "servo.h" #define rightIR 11 #define rearIR 5 #define frontIR 3 #define int1 28 // Направление вращения двигателя 1 #define int2 26 // Направление вращения двигателя 1 #define int3 24 // Направление вращения двигателя 2 #define int4 22 // Направление вращения двигателя 2 #define sp1 8 #define sp2 7 #define KursPin 10 #define TungPin 9 #define ForwardButton 'F' //0x5F12E8C4 //up button #define BackButton 'B' //0x189D7928 //down button #define RightButton 'R' //0x68733A46 //right button #define LeftButton 'L' //0x83B19366 //left button #define LeftSpeedUpButton 'D' //plus button #define LeftSpeedDownButton 'E' //minus button #define StopProgramm 'H' #define StartProgButton 'G'//0x89964248 //start button #define StopProgButton 'S'//0xE13DDA28 //stop button #define RightForwardButton 'C' #define LeftForwardButton 'A' #define RootPermission 'I' #define RootExit 'J' #define RightSpeedUpButton 'K' #define RightSpeedDownButton 'M' #define LostConnection 'N' #define SKursLeft 'T' #define SKursRight 'O' #define STungRight 'P' #define STungLeft 'Q' #define defaultpos 'U' #define EchoPin 4 #define TrigPin 2 #define ChangeSpeed 5 #define ChangeAngle 5 int save = 0; int speed1 = 255; int speed2 = 255; int rootflag = 0; int tungpos = 90; int kurspos = 90; int KursMaxAngle = 85; int TungMaxAngle = 35; Servo servokurs; //объявляем переменную Servo Servo servotung; void forward() { analogWrite(sp1, speed1); analogWrite(sp2, speed2); Serial.print(millis()); Serial.print("\t"); Serial.print("forward\n"); digitalWrite(int1, LOW); digitalWrite(int2, HIGH); digitalWrite(int3, HIGH); digitalWrite(int4, LOW); return; } void back() { analogWrite(sp1, speed1); analogWrite(sp2, speed2); Serial.print(millis()); Serial.print("\t"); Serial.print("back\n"); digitalWrite(int1, HIGH); digitalWrite(int2, LOW); digitalWrite(int3, LOW); digitalWrite(int4, HIGH); return; } void right() { Serial.print(millis()); Serial.print("\t"); Serial.print("right\n"); analogWrite(sp1, speed1); analogWrite(sp2, speed2); digitalWrite(int1, LOW); digitalWrite(int2, HIGH); digitalWrite(int3, LOW); digitalWrite(int4, HIGH); return; } void left() { Serial.print(millis()); Serial.print("\t"); Serial.print("left\n"); analogWrite(sp1, speed1); analogWrite(sp2, speed2); digitalWrite(int1, HIGH); digitalWrite(int2, LOW); digitalWrite(int3, HIGH); digitalWrite(int4, LOW); return; } void stop1() { Serial.print(millis()); Serial.print("\t"); Serial.print("stop\n"); digitalWrite(int1, LOW); digitalWrite(int2, LOW); digitalWrite(int3, LOW); digitalWrite(int4, LOW); return; } void rightforward() { analogWrite(sp1, speed1 - 150); analogWrite(sp2, speed2); Serial.print(millis()); Serial.print("\t"); Serial.print("rightforward\n"); digitalWrite(int1, LOW); digitalWrite(int2, HIGH); digitalWrite(int3, HIGH); digitalWrite(int4, LOW); return; } void leftforward() { analogWrite(sp1, speed1); analogWrite(sp2, speed1 - 150); Serial.print(millis()); Serial.print("\t"); Serial.print("leftworward\n"); digitalWrite(int1, LOW); digitalWrite(int2, HIGH); digitalWrite(int3, HIGH); digitalWrite(int4, LOW); return; } void servokursright() { Serial.println("kursright"); if ( kurspos < 90 + KursMaxAngle) { kurspos -= ChangeAngle; } servokurs.write(kurspos); Serial.println(kurspos); } void servokursleft() { Serial.println("kursleft"); if ( kurspos > 90 - KursMaxAngle) { kurspos += ChangeAngle; } servokurs.write(kurspos); Serial.println(kurspos); } void servotungright() { Serial.println("tungright"); if ( tungpos < 90 + TungMaxAngle) { tungpos += ChangeAngle; } servotung.write(tungpos); Serial.println(tungpos); } void servotungleft() { Serial.println("tungleft"); if ( tungpos > 90 - TungMaxAngle) { tungpos -= ChangeAngle; } servotung.write(tungpos); Serial.println(tungpos); } void DefaultPosition () { Serial.println("defpos"); servotung.write(90); servokurs.write(90); return; } void porog() { int fIR; int rIR; fIR = digitalRead(frontIR); rIR = digitalRead(rearIR); if ( fIR == 1 || rIR == 1) { if ( fIR == 1 ) { Serial.print(millis()); Serial.print("\t"); Serial.print("danger in front use left right or root\n"); back(); delay(300); stop1(); } if ( rIR == 1 ) { Serial.print(millis()); Serial.print("\t"); Serial.print("danger on back use left right or root\n"); forward(); delay(300); stop1(); } while (1) { if (Serial.available()) { int res; res = Serial.read(); if (res == RootPermission) { root(); return; } switch (res) { case RightButton: right(); return; break; case LeftButton: left(); return; break; case SKursRight: servokursright(); break; case SKursLeft: servokursleft(); break; case STungRight: servotungright(); break; case STungLeft: servotungleft(); break; case defaultpos: DefaultPosition (); break; } } } } } void KeySelect (int res) { switch (res) { case ForwardButton: forward(); break; case BackButton: back(); break; case RightButton: right(); break; case LeftButton: left(); break; case StopProgButton: stop1(); break; case RightForwardButton: rightforward(); break; case LeftForwardButton: leftforward(); break; case LostConnection: connection(); break; case SKursRight: servokursright(); break; case SKursLeft: servokursleft(); break; case STungRight: servotungright(); break; case STungLeft: servotungleft(); break; case defaultpos: DefaultPosition (); break; case LeftSpeedUpButton: if (speed1 < 254) { speed1 += ChangeSpeed; Serial.print(millis()); Serial.print("\t"); Serial.println(speed1); } break; case LeftSpeedDownButton: if (speed1 > 100) { speed1 -= ChangeSpeed; Serial.print(millis()); Serial.print("\t"); Serial.println(speed1); } break; case RightSpeedUpButton: if (speed1 < 254) { speed2 += ChangeSpeed; Serial.print(millis()); Serial.print("\t"); Serial.println(speed2); } break; case RightSpeedDownButton: if (speed1 > 100) { speed2 -= ChangeSpeed; Serial.print(millis()); Serial.print("\t"); Serial.println(speed2); } break; } return; } void root() { save = 0; Serial.print(millis()); Serial.print("\t"); Serial.print("root available\n"); while (1) { int res; res = Serial.read(); if (res == RootExit) { Serial.print(millis()); Serial.print("\t"); Serial.print ("root disable\n"); break; } KeySelect(res); } return; } void connection () { stop1(); Serial.print(millis()); Serial.print("\t"); Serial.print("lost_connection\n"); digitalWrite(39, HIGH); delay (2000); digitalWrite(39, LOW); } long getEchoTiming() { digitalWrite(TrigPin, LOW); delayMicroseconds(2); digitalWrite(TrigPin, HIGH); delayMicroseconds(10); digitalWrite(TrigPin, LOW); long duration = pulseIn(EchoPin, HIGH); return duration; } void setup() { pinMode(frontIR, INPUT); Serial.begin(9600); // Выставляем скорость COM порта Wire.begin(); servokurs.attach(KursPin); servotung.attach(TungPin); servokurs.write(kurspos); servotung.write(tungpos); pinMode(TrigPin, OUTPUT); pinMode(EchoPin, INPUT); pinMode(int1, OUTPUT); pinMode(int2, OUTPUT); pinMode(int3, OUTPUT); pinMode(int4, OUTPUT); pinMode(sp1, OUTPUT); pinMode(sp2, OUTPUT); pinMode(rightIR, INPUT); pinMode(rearIR, INPUT); } void loop() { if (Serial.available()) { int res; res = Serial.read(); if (res == RootPermission) { root(); } KeySelect(res); } porog(); }

описание программы для работы теле-управляемой подвижной платформой