DIY: делаем боевого робота в домашних условиях. Часть 2 «Epic Fail»
#include <math.h>
#define STEP_PIN1 12
#define STEP_PIN2 10
#define RELEPIN0 2
#define RELEPIN1 3
#define RELEPIN2 4
#define RELEPIN3 5
#define RELEPIN4 6
#define RELEPIN5 7
#define RELEPIN6 8
#define RELEPIN7 9
#define a0 = A0;
#define a1 = A1;
#define a2 = A2;
#define a3 = A3;
#define a4 = A4;
#define a5 = A5;
//Второй пульт
//Временного отключили второй пульт
//#define a6 = A8;
//#define a7 = A9;
int ch0,ch1,ch2,ch3,ch4,ch5,ch6,ch7;
//Задаем максимум и минимум для ШИМа с первого приемника
const int min0=1094;
const int min1=1094;
const int max0=813;
const int max1=813;
int speed1,speed2,speedmot1,speedmot2,sd1,sd2;
byte ch2st,ch3st,ch4st,ch5st,ch6st,ch7st;
byte tormozcount1=0;
byte tormozcount2=0;
byte wasdir1=2;
byte wasdir2=2;
byte skolkotormozit = 7;
int skolkotormozim=0;
void setup() {
Serial.begin(9600);
Serial.println("start");
pinMode(a0, INPUT);
pinMode(a1, INPUT);
pinMode(a2, INPUT);
pinMode(a3, INPUT);
pinMode(a4, INPUT);
pinMode(a5, INPUT);
//Временного отключили второй пульт
//pinMode(a6, INPUT);
//pinMode(a7, INPUT);
pinMode(STEP_PIN1, OUTPUT);
pinMode(STEP_PIN2, OUTPUT);
pinMode(RELEPIN0, OUTPUT);
pinMode(RELEPIN1, OUTPUT);
pinMode(RELEPIN2, OUTPUT);
pinMode(RELEPIN3, OUTPUT);
pinMode(RELEPIN4, OUTPUT);
pinMode(RELEPIN5, OUTPUT);
pinMode(RELEPIN6, OUTPUT);
pinMode(RELEPIN7, OUTPUT);
}
void loop() {
ch0 =pulseIn(a0, HIGH,30000);
if(ch0<900){
//Если нет сигнала с пульта
Serial.println("first no signal");
digitalWrite(STEP_PIN1,LOW);
digitalWrite(STEP_PIN2,LOW);
digitalWrite(RELEPIN0,LOW);
digitalWrite(RELEPIN1,LOW);
digitalWrite(RELEPIN2,LOW);
digitalWrite(RELEPIN3,LOW);
digitalWrite(RELEPIN4,LOW);
digitalWrite(RELEPIN5,LOW);
digitalWrite(RELEPIN6,LOW);
digitalWrite(RELEPIN7,LOW);
}
//Снимаем показания с ШИМа от первого приемника
ch1 =pulseIn(a1, HIGH);
ch2 =pulseIn(a2, HIGH);
ch3 =pulseIn(a3, HIGH);
ch4 =pulseIn(a4, HIGH);
ch5 =pulseIn(a5, HIGH);
//Временного отключили второй пульт
ch6 =1250; //pulseIn(a6, HIGH,2020);
ch7 =1250; //pulseIn(a7, HIGH,2020);
if(ch2>1500){
ch2st=1;
}else{
ch2st=0;
}
if(ch3>1500){
ch3st=0;
}else if(ch3>0){
ch3st=1;
}
if(ch4>1500){
ch4st=1;
}else{
ch4st=0;
}
if(ch5>1500){
ch5st=1;
}else{
ch5st=0;
}
//Каждый канал второго пульта управляет двумя орудиями
if(ch6>1800){
ch6st=2;
}else if(ch6<1200){
ch6st=1;
}else{
ch6st=0;
}
if(ch7>1800){
ch7st=2;
}else if(ch7<1200){
ch7st=1;
}else{
ch7st=0;
}
//Первое орудие----------------------------------------------
if(ch2st==1 or ch6st==1){
digitalWrite(RELEPIN0,HIGH);
}else{
digitalWrite(RELEPIN0,LOW);
}
//Второе орудие----------------------------------------------
if(ch3st==1 or ch6st==2){
digitalWrite(RELEPIN1,HIGH);
}else {
digitalWrite(RELEPIN1,LOW);
}
//Третье орудие----------------------------------------------
if(ch4st==1 or ch7st==1){
digitalWrite(RELEPIN2,HIGH);
}else {
digitalWrite(RELEPIN2,LOW);
}
//Если есть сигнал с первого пульта
if(ch0>0){
ch0=(ch0<55 && ch0>45)?50:ch0;
ch1=(ch1<55 && ch1>45)?50:ch1;
ch0 =ch0-min0;
ch1 =ch1-min1;
//Получаем данные о положении стиков управления от 0 до 100
ch0 =map(ch0,0,max0,0,100);
ch1 =100-map(ch1,0,max1,0,100);
//Превращаем данные о напралении движения в необходимые значения для каждого двигателя
speed1=round((ch0-50)*(abs(ch1-50)+50)/50);
speed2=round((ch1-50)*(abs(ch0-50)+50)/50);
//корректируем значения и считаем скорость моторов
speedmot1=speed1+speed2;
speedmot2=speed1-speed2;
// Выставляем ограничения
speedmot1=(speedmot1>100)?100:speedmot1;
speedmot2=(speedmot2>100)?100:speedmot2;
speedmot1=(speedmot1<-100)?-100:speedmot1;
speedmot2=(speedmot2<-100)?-100:speedmot2;
speedmot1=(speedmot1<20 && speedmot1>-20)?0:speedmot1;
speedmot2=(speedmot2<20 && speedmot2>-20)?0:speedmot2;
sd1=map(abs(speedmot1),0,100,0,255);
sd2=map(abs(speedmot2),0,100,0,255);
//Вывод значений для диагностики
Serial.print(ch0);
Serial.print(";");
Serial.print(ch1);
Serial.print(";");
Serial.print(";");
Serial.print(speed1);
Serial.print(";");
Serial.print(speed2);
Serial.print(";");
Serial.print(speedmot1);
Serial.print(";");
Serial.print(speedmot2);
Serial.print(";");
Serial.print(sd1);
Serial.print(";");
Serial.print(sd2);
Serial.print(";wasdir");
Serial.print(wasdir1);
Serial.print(";");
Serial.print(wasdir2);
Serial.println(";");
//Выдаем ШИМ для драйвера, если мы не в состоянии торможения
if(tormozcount1==0 and tormozcount2==0){
if(sd1<220){
analogWrite(STEP_PIN1,sd1);
}else{
digitalWrite(STEP_PIN1,HIGH);
}
if(sd2<220){
analogWrite(STEP_PIN2,sd2);
}else{
digitalWrite(STEP_PIN2,HIGH);
}
}else{
digitalWrite(STEP_PIN1,LOW);
digitalWrite(STEP_PIN2,LOW);
}
//торможение, выбег ротора
if(speedmot1<40 and speedmot1>-40){
Serial.println("Tormoz menshe 40 1 dvig;");
digitalWrite(RELEPIN3,HIGH);
digitalWrite(STEP_PIN1,LOW);
tormozcount1=0;
}else{
digitalWrite(RELEPIN3,LOW);
}
if(speedmot2<40 and speedmot2>-40 ){
Serial.println("Tormoz menshe 40 2 dvig;");
digitalWrite(RELEPIN4,HIGH);
digitalWrite(STEP_PIN2,LOW);
tormozcount2=0;
}else{
digitalWrite(RELEPIN4,LOW);
}
//Если мы тормозим больше циклов, чем нужно для тормоза считаем, что мы неподвижны
if(speedmot1<40 and speedmot1>-40 and speedmot2<40 and speedmot2>-40){
skolkotormozim++;
}else{
skolkotormozim=0;
}
if( skolkotormozim>skolkotormozit){
wasdir1=2;
wasdir2=2;
digitalWrite( RELEPIN5, LOW);
digitalWrite( RELEPIN6, LOW);
Serial.println("Stop for 10 cicles");
}
//смена направления хода
if((speedmot1 < -10 and wasdir1==1) || (speedmot1 >=5 and wasdir1==0)){
//Если новое направление не соответствует старому
if(tormozcount1<=0){
//если счетчик 0 тормозим
Serial.println("Tormoz vibeg rotora 1 dvig;");
digitalWrite(STEP_PIN1,LOW);
digitalWrite(RELEPIN3,HIGH);
tormozcount1=skolkotormozit;
}
if(tormozcount1>0){
//Если уже тормозим
digitalWrite(STEP_PIN1,LOW);
tormozcount1--;
}
if(tormozcount1==1){
//если оттормозили
tormozcount1--;
digitalWrite(RELEPIN3,LOW);
Serial.println("Change 1dvig ");
if(speedmot1 < 0){
wasdir1=0;
digitalWrite( RELEPIN5, HIGH);
Serial.println("vzad");
}else{
wasdir1=1;
digitalWrite( RELEPIN5, LOW);
Serial.println("vpered");
}
}
}
if((speedmot2 < -10 and wasdir2==1) || (speedmot2 >=5 and wasdir2==0)){
//Если новое направление не соответствует старому
if(tormozcount2<=0){
//если счетчик 0 тормозим
Serial.println("Tormoz vibeg rotora 2 dvig;");
digitalWrite(STEP_PIN2,LOW);
digitalWrite(RELEPIN4,HIGH);
tormozcount2=skolkotormozit;
}
if(tormozcount2>0){
//Если уже тормозим
digitalWrite(STEP_PIN2,LOW);
tormozcount2--;
}
if(tormozcount2==1){
//если оттормозили
tormozcount2--;
digitalWrite(RELEPIN4,LOW);
Serial.print("Change 2dvig ");
if(speedmot2 < 0){
wasdir2=0;
digitalWrite( RELEPIN6, HIGH);
Serial.println("vzad");
}else{
wasdir2=1;
digitalWrite( RELEPIN6, LOW);
Serial.println("vpered");
}
}
}
// Если долго никуда не ехали меняем направление сразу
if(speedmot1 < -40 and wasdir1==2){
wasdir1=0;
digitalWrite( RELEPIN5, HIGH);
Serial.println("Reverse 1 motor");
}
if(speedmot1 > 40 and wasdir1==2){
wasdir1=1;
digitalWrite( RELEPIN5, LOW);
Serial.println("Forward 1 motor");
}
if(speedmot2 < -40 and wasdir2==2){
wasdir2=0;
digitalWrite( RELEPIN6, HIGH);
Serial.println("Reverse 2 motor");
}
if(speedmot2 > 40 and wasdir2==2){
wasdir2=1;
digitalWrite( RELEPIN6, LOW);
Serial.println("Forward 2 motor");
}
/*
//Диагностика
Serial.print(tormozcount1);
Serial.print(";");
Serial.print(tormozcount2);
Serial.print(";");
Serial.print(wasdir1);
Serial.print(";");
Serial.print(wasdir2);
Serial.print(";");
*/
//delay(1000);
}else{
//Если нет сигнала с пульта
}
}
