#include Wire.h
#include utility/twi.h
#include EEPROM.h
#include "Nunchuk.h"
Nunchuk chuk;
int VM1 = 10; //viteza motor 1
int VM2 = 11; //viteza motor 2
int DM1 = 12; //directie motor 1
int DM2 = 13; //directie motor 2
int x = 0; //axa x joystick
int y = 0; //axa y joystick
int stare_buton_Z = 1; // variabila pentru citirea starii butonului Z
int stare_led1 = 0; // variabila aratand starea led-ului
int schimbare1 = 0;
int stare_buton_C = 1; // variabila pentru citirea starii butonului C
int stare_led2 = 0; // variabila aratand starea led-ului
int schimbare2 = 0;
int SenS = 8; // senzor stanga
int SenD = 9; // senzor dreapta
int Directie = 0;
int viteza = 0;
int VitezaSpate = 0;
int VitezaStanga = 0;
int VitezaDreapta = 0;
void setup()
{
pinMode(10, OUTPUT); //viteza motor 1
pinMode(11, OUTPUT); //viteza motor 2
pinMode(12, OUTPUT); //directie motor 1
pinMode(13, OUTPUT); //directie motor 2
pinMode(6, OUTPUT); //pin led 1
pinMode(7, OUTPUT); //pin led 2
pinMode(SenS, INPUT); //intrare senzor stanga
pinMode(SenD, INPUT); //intrare senzor dreapta
Serial.begin(115200);
Wire.begin();
// use 400 kHz for the nunchuck, because it doesn't work reliable with 100 kHz
TWBR = ((CPU_FREQ / 400000L) - 16) / 2;
delay(100);
}
void loop()
{
if(!chuk.ok())
{
//Serial.println("Initing nunchuk, it's broken!");
chuk.init();
}
if(chuk.read())
{
//Serial.print(chuk.joyX(), DEC);
//Serial.write(' ');
//Serial.print(chuk.joyY(), DEC);
//Serial.write(' ');
//Serial.print(chuk.buttonZ(), DEC);
//Serial.write(' ');
//Serial.print(chuk.buttonC(), DEC);
//Serial.write(' ');
//Serial.print(chuk.accX(), DEC);
//Serial.write(' ');
//Serial.print(chuk.accY(), DEC);
//Serial.write(' ');
//Serial.print(chuk.accZ(), DEC);
//Serial.println();
}
else
{
//Serial.println("chuck.read() failed");
}
stare_buton_Z = chuk.buttonZ();
if (stare_buton_Z == 0) { // Verifica daca butonul Z este apasat
// Yes, input is 0
// If LED is on, turn it off
// If LED is off, turn it on
if ((stare_led1 == 0) && (schimbare1 == 0)){
digitalWrite(6, HIGH); // "Aprinde" led-ul, control din joystick
stare_led1 = 1;
schimbare1 = 1; // Set change, prevent new change
}
if ((stare_led1 == 1) && (schimbare1 == 0)){
digitalWrite(6, LOW); // "Stinge" led-ul, control din accelerometru
stare_led1 = 0;
schimbare1 = 1; // Set change, prevent new change
}
}
else { // Butonul Z nu este apasat,
schimbare1 = 0; // intrarea este HIGH din nou, reseteaza "schimbare",
}
stare_buton_C = chuk.buttonC();
if (stare_buton_C == 0) { // Verifica daca butonul C este apasat
// Yes, input is 0
// If LED is on, turn it off
// If LED is off, turn it on
if ((stare_led2 == 0) && (schimbare2 == 0)){
digitalWrite(7, HIGH); // "Aprinde" led-ul, control manual
stare_led2 = 1;
schimbare2 = 1; // Set change, prevent new change
}
if ((stare_led2 == 1) && (schimbare2 == 0)){
digitalWrite(7, LOW); // "Stinge" led-ul, control automat
stare_led2 = 0;
schimbare2 = 1; // Set change, prevent new change
}
}
else { // Butonul C nu este apasat,
schimbare2 = 0; // intrarea este HIGH din nou, reseteaza "schimbare",
}
if(stare_led1 == 0){ // Control din accelerometru
x = (chuk.accX());
y = (chuk.accY());
x = constrain(x, 300, 700); //restrângem valorile pe axa x
//if (x >= 573)
if (x >= 550)
{
//x = map(x, 573, 700, 0, 127);
x = map(x, 550, 700, 30, 127);
}
//else if (x <= 427)
else if (x <= 450)
{
// x = map(x, 300, 427, -127, 0);
x = map(x, 300, 450, -127, 30);
}
else
{
x = 0;
}
y = constrain(y, 300, 700);
//if (y >= 573)
if (y >= 550)
{
//y = map(y, 573, 700, 0, 127);
y = map(y, 550, 700, 30, 127);
}
//else if (y <= 427)
else if (y <= 450)
{
//y = map(y, 300, 427, -127, 0);
y = map(y, 300, 450, -127, 30);
}
else
{
y = 0;
}
}
if(stare_led1 == 1){ // Control din joystick
x = ((chuk.joyX()) - 1); // Aducere la 0 a axei x
y = (chuk.joyY());
}
Serial.print("X = ");
Serial.print(x, DEC);
Serial.write(' ');
Serial.print("Y = ");
Serial.print(y, DEC);
Serial.println();
if(stare_led2 == 1){ // daca led-ul 2 este "aprins" control manual
viteza = 0; //resetam viteza automata la 0
VitezaSpate = 0;
VitezaStanga = 0;
VitezaDreapta = 0;
if ( x == 0 && y == 0) // daca x si y = 0
{
digitalWrite(VM1, LOW); // opreste motoarele
digitalWrite(VM2, LOW);
}
else if (y >= 0) //daca y >= 0
{
int sf = ((2 * y) + x); //stanga fata
sf = constrain(sf, -255, 255);
int df = ((2 * y) - x); //dreapta fata
df = constrain(df, -255, 255);
Serial.print("SF = ");
Serial.print(sf, DEC);
Serial.write(' ');
Serial.print("DF = ");
Serial.print(df, DEC);
Serial.println();
if (sf > 0) // daca stanga fata > 0
{
digitalWrite(DM1, LOW); // porneste motorul 1 in fata
analogWrite(VM1, sf); // viteza motor 1
}
else if (sf < 0) // daca stanga fata < 0
{
digitalWrite(DM1, HIGH); // porneste motorul 1 in spate
analogWrite(VM1, (map(sf, 0, -255, 0, 255))); // viteza motor 1
}
if (df > 0) // daca dreapta fata > 0
{
digitalWrite(DM2, LOW); // porneste motorul 2 in fata
analogWrite(VM2, df); // viteza motor 2
}
else if (df < 0) // daca dreapta fata < 0
{
digitalWrite(DM2, HIGH); // porneste motorul 2 in spate
analogWrite(VM2, (map(df, 0, -255, 0, 255))); // viteza motor 2
}
}
else if (y < 0) //daca < 0
{
int ss = (-y + x); //stanga spate
ss = constrain(ss, -255, 255);
int ds = (-y - x); //dreapta spate
ss = constrain(ss, -255, 255);
Serial.print("SS = ");
Serial.print(ss, DEC);
Serial.write(' ');
Serial.print("DS = ");
Serial.print(ds, DEC);
Serial.println();
if (ss > 0) // daca stanga spate > 0
{
digitalWrite(DM1, HIGH); // porneste motorul 1 in spate
analogWrite(VM1, ss); // viteza motor 1
}
else if (ss < 0) // daca stanga spate < 0
{
digitalWrite(DM1, LOW); // porneste motorul 1 in fata
analogWrite(VM1, (map(ss, 0, -255, 0, 255))); // viteza motor 1
}
if (ds > 0) // daca dreapta spate > 0
{
digitalWrite(DM2, HIGH); // porneste motorul 2 in spate
analogWrite(VM2, ds); // viteza motor 2
}
else if (ds < 0) // daca dreapta spate < 0
{
digitalWrite(DM2, LOW); // porneste motorul 2 in fata
analogWrite(VM2, (map(ds, 0, -255, 0, 255))); // viteza motor 2
}
}
}
else if(stare_led2 == 0){ //daca led-ul 2 este "stins" control automat
if ( digitalRead(SenS) == 1 && digitalRead(SenD) == 1 && Directie == 0) { //mers in fata
VitezaSpate = 0; //resetam viteza spate la 0
VitezaStanga = 0; //resetam stanga spate la 0
VitezaDreapta = 0; //resetam dreapta spate la 0
digitalWrite(DM1, LOW); // porneste motorul 1 in fata
digitalWrite(DM2, LOW); // porneste motorul 2 in fata
viteza += 1;
viteza = min(viteza, 200); // viteza nu va depasi 200
analogWrite(VM1, viteza); // viteza motor 1
analogWrite(VM2, viteza); // viteza motor 2
Serial.print("Viteza 1 = ");
Serial.print(viteza, DEC);
Serial.write(' ');
Serial.print("Viteza 2 = ");
Serial.print(viteza, DEC);
Serial.println();
delay(5);
}
else if ( digitalRead(SenS) == 0 && digitalRead(SenD) == 0) {//daca este un obstacol in fata
viteza = 0; //setam viteza automata la 0
VitezaStanga = 0; //resetam stanga spate la 0
VitezaDreapta = 0; //resetam dreapta spate la 0
Directie = 1;
}
else if ( digitalRead(SenS) == 0 && digitalRead(SenD) == 1) {//daca este un obstacol in stanga
viteza = 0; //setam viteza automata la 0
VitezaSpate = 0; //resetam viteza spate la 0
VitezaDreapta = 0; //resetam dreapta spate la 0
Directie = 2;
}
else if ( digitalRead(SenS) == 1 && digitalRead(SenD) == 0) {//daca este un obstacol in dreapta
viteza = 0; //setam viteza automata la 0
VitezaStanga = 0; //resetam stanga spate la 0
VitezaSpate = 0; //resetam viteza spate la 0
Directie = 3;
}
if (Directie == 1) //mers in spate
{
digitalWrite(DM1, HIGH); // porneste motorul 1 in spate
digitalWrite(DM2, HIGH); // porneste motorul 2 in spate
VitezaSpate += 1;
VitezaSpate = min(VitezaSpate, 200); // viteza nu va depasi 200
analogWrite(VM1, VitezaSpate); // viteza motor 1
analogWrite(VM2, VitezaSpate / 2); // viteza motor 2
if (VitezaSpate == 200) {
Directie = 0;
}
//delay(5);
//Serial.print("Viteza 1 = ");
//Serial.print(VitezaSpate, DEC);
//Serial.write(' ');
//Serial.print("Viteza 2 = ");
//Serial.print(VitezaSpate / 2, DEC);
//Serial.println();
}
else if (Directie == 2) //
{
//digitalWrite(DM1, LOW); // porneste motorul 1 in fata
digitalWrite(DM1, HIGH); // porneste motorul 1 in spate
digitalWrite(DM2, HIGH); // porneste motorul 2 in spate
VitezaStanga += 1;
VitezaStanga = min(VitezaStanga, 200); // viteza nu va depasi 200
analogWrite(VM1, VitezaStanga); // viteza motor 1
analogWrite(VM2, VitezaStanga / 2); // viteza motor 2
if (VitezaStanga == 200) {
Directie = 0;
}
//delay(5);
}
else if (Directie == 3) //
{
digitalWrite(DM1, HIGH); // porneste motorul 1 in spate
digitalWrite(DM2, HIGH); // porneste motorul 2 in spate
//digitalWrite(DM2, LOW); // porneste motorul 2 in fata
VitezaDreapta += 1;
VitezaDreapta = min(VitezaDreapta, 200); // viteza nu va depasi 200
analogWrite(VM1, VitezaDreapta / 2); // viteza motor 1
analogWrite(VM2, VitezaDreapta); // viteza motor 2
if (VitezaDreapta == 200) {
Directie = 0;
}
//delay(5);
//Serial.print("Viteza 1 = ");
//Serial.print(VitezaDreapta, DEC);
//Serial.write(' ');
//Serial.print("Viteza 2 = ");
//Serial.print(VitezaDreapta / 2, DEC);
//Serial.println();
}
}
Serial.print("Directie = ");
Serial.print(Directie, DEC);
Serial.println();
delay(5);
}
Nunchuk remote operated car
Subscribe to:
Posts
(
Atom
)
No comments :
Post a Comment