Skip to content

Commit

Permalink
Arduino
Browse files Browse the repository at this point in the history
  • Loading branch information
@erusbot
erusbot authored Sep 28, 2018
1 parent a6b75d2 commit d48e5b3
Showing 1 changed file with 259 additions and 0 deletions.
259 changes: 259 additions & 0 deletions arduino/arduino.ino
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,259 @@
#include <TimerOne.h>
//#include <SoftPWM.h>

#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);
}
}
}

0 comments on commit d48e5b3

Please sign in to comment.