by adrianovrm » Wed 26. Dec 2012, 23:06
/*
Meu código para rodar a maquete do youtube que demonstrei no link passado
4.294.967.294 bilhoes de posicoes
Deve ficar num range de 19 posicoes - divisor igual a = 226050910.2105263 que ficaram no intervalo de
X~a01~Y~a02~
*/
#include <LiquidCrystal.h>
#include <Servo.h>
const int numRows = 2;
const int numCols = 16;
int A = 2;
int B = 3;
int C = 4;
int P = 5;
int D = 6;
int E = 7;
int F = 8;
int G = 9;
int inByte;
LiquidCrystal lcd(12, 11, A0, A1, A2, A3);
int rowsel;
int buffer[14];
int counteri;
char tempstr[10];
int gearsmap = 0;
unsigned int carspeed;
unsigned int rpm;
int pspeed;
byte d1;
byte d2;
byte gaugerpm;
//Servo servo1;
//Servo servo2;
Servo servoFR; //defenir os nomes dos servos
Servo servoFL;
Servo servoRR; //defenir os nomes dos servos
Servo servoRL;
int FG_longintudinal;
int FG_lateral;
void setup() {
// initialize the digital pin as an output.
// Pin 13 has an LED connected on most Arduino boards:
//Serial.begin(9600);
Serial.begin(9600);
lcd.begin(numCols,numRows);
pinMode(A, OUTPUT);
pinMode(B, OUTPUT);
pinMode(C, OUTPUT);
pinMode(P, OUTPUT);
pinMode(D, OUTPUT);
pinMode(E, OUTPUT);
pinMode(F, OUTPUT);
pinMode(G, OUTPUT);
pinMode(A0, OUTPUT);
pinMode(A1, OUTPUT);
pinMode(A2, OUTPUT);
pinMode(A3, OUTPUT);
pinMode(A4, OUTPUT);
pinMode(A5, OUTPUT);
pinMode(5,OUTPUT);
pinMode(6,OUTPUT);
pinMode(9,OUTPUT);
pinMode(10,OUTPUT);
//escrevadigito(-8);
//digitalWrite(A0, LOW);
//delay(2000);
//rowsel = 0 ;
//servo1.attach(A4);
//servo1.write(0);
servoRR.attach(5);
servoFR.attach(6);
servoFL.attach(9);
servoRL.attach(10);
//servoFR.write(180); // servos iniciao na posição 0
//servoFL.write(0);
//servoRR.write(180); // servos iniciao na posição 0
//servoRL.write(0);
servoFR.write(90); // servos iniciao na posição 0
servoFL.write(90);
servoRR.write(90); // servos iniciao na posição 0
servoRL.write(90);
}
void loop() {
if (Serial.available() > 10)
{
counteri = 0;
for (counteri = 0; counteri < 10; counteri++)
{
buffer[counteri] = Serial.read();
}
if (buffer[0] == 88)
{
lcd.setCursor(0,0);
lcd.print(" ");
lcd.setCursor(0,1);
lcd.print(" ");
lcd.setCursor(0,0);
//escrevadigito(1);
//lcd.println(buffer[3]);
lcd.print('X');
lcd.setCursor(1,0);
lcd.print(buffer[1]);
lcd.setCursor(5,0);
lcd.print('Y');
lcd.setCursor(6,0);
lcd.print(buffer[2]);
lcd.setCursor(10,0);
lcd.print('G');
lcd.setCursor(11,0);
gearsmap = map(buffer[3],111,255,-1,8);
lcd.print(gearsmap);
lcd.setCursor(12,0);
lcd.print('S');
lcd.setCursor(13,0);
d1 = buffer[4]; // store high byte of speed
d2 = buffer[5]; // store low byte of speed
carspeed = d1 * 256 + d2;
carspeed = map(carspeed,32639, 65536,0,400);
//lcd.print(map(carspeed,224,255,0,450)); // carspeed
lcd.print(carspeed);
// carspeed(value,127,577,0,450);
lcd.setCursor(0,1);
lcd.print('R');
lcd.setCursor(1,1);
d1 = buffer[6]; // store high byte of speed
d2 = buffer[7]; // store low byte of speed
rpm = d1 * 256 + d2;
rpm = map(rpm,33127, 65536,0,10000);
lcd.print(rpm);
gaugerpm =0;
lcd.setCursor(7,1);
lcd.print('O');
lcd.setCursor(8,1);
d2 = buffer[8]; // valor aceleracao longitudinal
FG_longintudinal = 0;
//FG_longintudinal = map (d2,117,137,-10,10);
FG_longintudinal = map(d2,0,255,-10,10);
lcd.print(FG_longintudinal);
// mesmas variaveis - forca lateral
d2 = buffer[9]; // valor aceleracao lateral
FG_lateral = 0;
FG_lateral = map(d2,0,255,-10,10);
lcd.setCursor(11,1);
lcd.print('A');
lcd.setCursor(12,1);
lcd.print(FG_lateral);
atualiza_servo(FG_longintudinal,FG_lateral);
}
}
}
float mapfloat(float val, float i1, float i2, float o1, float o2)
{
return o1+(o2-o1)*((val-i1)/(i2-i1));
}
void atualiza_servo(int f_longitudinal, int f_lateral)
{
int angulo_servoRL;
int angulo_servoRR;
int angulo_servoFL;
int angulo_servoFR;
// servo RL diminuir angulos
// servo RR aumenta angulo
// servo1.write(0); // servo1 vai para 180 graus
// servo2.write(180);
//angulo_servo1 = map(f_longitudinal,-4, 4,0,179);
// map(f_longitudinal,-10, 10,0,179);
angulo_servoRR = f_longitudinal*15 + 90;
angulo_servoRL = 180 - angulo_servoRR;
angulo_servoFR = -f_longitudinal*15 + 90;
angulo_servoFL = 180 - angulo_servoFR;
if (f_lateral > 0) //dobrando a esquerda
{
angulo_servoFR = angulo_servoFR + f_lateral*15;
angulo_servoRR = angulo_servoRR + f_lateral*15;
}
else if (f_lateral <0) //dobrando a direita
{
angulo_servoFL = angulo_servoFL + f_lateral*15;
angulo_servoRL = angulo_servoRL + f_lateral*15;
}
// se algum angulo passar de 180. voltar para 180
angulo_servoRL = map(angulo_servoRL,0,180,0,180);
angulo_servoRR = map(angulo_servoRR,0,180,0,180);
angulo_servoFR = map(angulo_servoFR,0,180,0,180);
angulo_servoFL = map(angulo_servoFL,0,180,0,180);
servoFR.write(angulo_servoFR); // servoFR iniciou na posição 0 ,sendo atualizado com nova posicao
servoFL.write(angulo_servoFL); // servosFL iniciou na posição 180,sendo atualizado com nova posicao
servoRR.write(angulo_servoRR); // servosRR iniciiou na posição 0,sendo atualizado com nova posicao
servoRL.write(angulo_servoRL); // servosRL que iniciou na posição 180,sendo atualizado com nova posicao
}