From d48e5b31c870550779f5512fba72c91660889808 Mon Sep 17 00:00:00 2001 From: ERUS Date: Fri, 28 Sep 2018 06:56:24 -0300 Subject: [PATCH] Arduino --- arduino/arduino.ino | 259 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 259 insertions(+) create mode 100644 arduino/arduino.ino diff --git a/arduino/arduino.ino b/arduino/arduino.ino new file mode 100644 index 0000000..709c192 --- /dev/null +++ b/arduino/arduino.ino @@ -0,0 +1,259 @@ +#include +//#include + +#define ID '2' //ID do robô, sendo '3' = atk, '2' = def, '1' = goleiro + +#define ledPin 13 //Led indicativo de bateria fraca porta D13 + +#define pwm_left 9 //definindo pinos para ajustar a velocidade do motor +#define pwm_right 6 + +#define dir_left 8 +#define dir_right 11 + +//Constantes do PI +//Formato: kp + ki*z/(z-1) +#define kp 12.79 +#define ki 1 + +volatile int D; +volatile int E; +volatile int vel; + +int dir_left_value; +int dir_right_value; + + +#define NOID 0xFF +#define ALLID 0x00 + +int flag; +long mill; +int pwmR, pwmL; +int pwmRForcado, pwmLForcado; +float veloc; +float velE; +float velD; +volatile double aux; //constante de velocidade + +int paused = 0; +int usandoPWM = 0; + +void rodaD (){ //incrementando a roda direita + D++; +} + +void rodaE (){ //incrementando a roda esquerda + E++; +} + +void TimeInt (){ + vel = 1; +} + +int mapVelPWM(float vel) //Mapeamento da velocidade +{ + int v = vel*100; + int pwm = 0; + if(v <= 0) + return 0; + if(v <= 117) + return map(v,0,117,16,19); + if(v <= 325) + return map(v,117,325,19,32); + if(v <= 443) + return map(v,325,443,32,44); + if(v <= 582) + return map(v,443,582,44,80); + return 80; +} + +double sumErroR = 0, sumErroL = 0; + +double pi_right(float setpoint, float medicao) +{ + double erro = setpoint - medicao; + sumErroR += erro; + return kp*erro + ki*sumErroR; +} + +double pi_left(float setpoint, float medicao) +{ + double erro = setpoint - medicao; + sumErroL += erro; + return kp*erro + ki*sumErroL; +} + +float mapPWMvel(int pwm) //??? +{ + +} + +void setup () { + //pinMode(ledPin, OUTPUT); + //pinMode(LED_BUILTIN, OUTPUT); + //digitalWrite(ledPin,LOW); + //digitalWrite(LED_BUILTIN, LOW); + Serial.begin(4800); + attachInterrupt(0, rodaE, CHANGE); + attachInterrupt(1, rodaD, CHANGE); + Timer1.initialize(10000); //tempo entre as interrupçes + Timer1.attachInterrupt(TimeInt); + aux = 0.133333; + pinMode(dir_right, OUTPUT); //configurando o pino 'dir_right' como saida + pinMode(pwm_right, OUTPUT); //configurando o pino 'pwm_right' como saída + pinMode(dir_left, OUTPUT); //configurando o pino 'dir_left' como saída + pinMode(pwm_left, OUTPUT); //configurando o pino 'pwm_left' como saída + + flag = pwmR = pwmL = 0; + + digitalWrite(dir_right,LOW); //configurando o pino 'dir_right' como low + digitalWrite(dir_left,HIGH); //configurando o pino 'dir_left' como high + //while(Serial.read() != 's') {} + mill = millis(); + D = 0; + Timer1.pwm(pwm_left,0); + analogWrite(pwm_right,0); //configurando o pino 'pwm_right' com valor pwm 0 +} + +float setpoint_right; +float setpoint_left; + +void loop () { + char inicio = 1; + if(Serial.available() >= 4){ + inicio = Serial.read(); //Lê o valor do primeiro comando, pegando o valor como um char e convertendo para inteiro. + } + if(inicio == 0){ + //if(Serial.available() >= 3) // Sem time out, estou confiando no protocolo zigbee e na placa. + //{ + //if(digitalRead(LED_BUILTIN) == HIGH){ + + // digitalWrite(LED_BUILTIN,LOW); + //}else{ + + //digitalWrite(LED_BUILTIN,HIGH); + + //} + //delay(100); + //digitalWrite(LED_BUILTIN,HIGH); + char cmd; + signed int value; + char id = Serial.read(); //Lê o ID do robô, podendo ser '1', '2', ou '3', para goleiro, zagueiro e atacante respectivamente. + cmd = Serial.read(); //Lê o comando do robô, sendo 'l', 'r', 'L', 'R' ou 's', explicação de cada comando no switch + value = Serial.read(); //Lê o valor do comando, pegando o valor como um char e convertendo para inteiro. + + + if(id == ID){//Confere qual o numero do robô + switch(cmd) + { + + case 'l': // Comando pwm da roda esquerda + { + paused = 0; + usandoPWM = 0; + value = (value-128)*2; + + if(value < 0) { + value *= -1; + dir_left_value = 1; + } else { + dir_left_value = 0; + } + setpoint_left = 5*value/255.0; + if( value == 0) + sumErroL = 0; + break; + } + case 'r': // Comando pwm da roda direita + { + paused = 0; + usandoPWM = 0; + value = (value-128)*2; + + if(value < 0) { + value *= -1; + dir_right_value = 0; + } else { + dir_right_value = 1; + } + setpoint_right = 5*value/255.0; + if( value == 0) + sumErroR = 0; + break; + } + case 'L': // Comando de forçar pwm da roda esquerda + { + paused = 0; + usandoPWM = 1; + value = (value-128)*2; + + if(value < 0) { + value *= -1; + dir_left_value = 1; + } else { + dir_left_value = 0; + } + pwmLForcado = value; + break; + } + + case 'R': // Comando de forçar pwm da roda direita + { + paused = 0; + usandoPWM = 1; + value = (value-128)*2; + if(value < 0) { + value *= -1; + dir_right_value = 0; + } else { + dir_right_value = 1; + } + pwmRForcado = value; + break; + } + + case 's': // Comando de pausa + { + //digitalWrite(ledPin,HIGH); + setpoint_right = 0; + setpoint_left = 0; + sumErroR = 0; + sumErroL = 0; + paused = 1; + break; + } + } + //}else if (id != '1' && id != '3'){ + // //digitalWrite(ledPin,HIGH); + //} + } + + if (vel == 1) // + { + vel = 0; + velD = D*aux; + velE = E*aux; + E = 0; + D = 0; + if(usandoPWM){ + pwmR = pwmRForcado; + pwmL = pwmLForcado; + }else{ + pwmR = mapVelPWM(pi_right(setpoint_right, velD)); + pwmL = 4*mapVelPWM(pi_left(setpoint_left, velE)); + } + } + if(paused) + { + analogWrite(pwm_right, 0); //fazendo o pino pwm_right ficar com o valor 0 + Timer1.pwm(pwm_left, 0); + } else { + analogWrite(pwm_right, pwmR); + Timer1.pwm(pwm_left, pwmL); + digitalWrite(dir_right,dir_right_value); //Se não pausar, setar o valor dir_right_value no pino dir_right + digitalWrite(dir_left,dir_left_value); + } + } +} +