Eg får vel legge ut koden då
Slik det er koda no, brukar eg ikkje den ekstra arduinoen.
TX
Kode
// Laste inn bibliotek
#include <nRF905.h>
#include <nRF905_config.h>
#include <nRF905_defs.h>
#include <nRF905_types.h>
#include <SPI.h>
//Lage RX/TX-addresser
#define RXADDR 0x44332211
#define TXADDR 0x99887766
//Deklarer I/O
const int fram_knapp = A0;
const int bak_knapp = A1;
const int venstre_knapp = A2;
const int hogre_knapp = A3;
const int mode1_bryter = 0;
const int mode2_bryter = 1;
const int fart_pot = A5;
const int acc_x = 5;
const int acc_y = 6;
void setup()
{
//Start nRF905
nRF905_init();
//Sett RX/TX-addresser
nRF905_setRXAddress(RXADDR);
nRF905_setTXAddress(TXADDR);
//Sett pinModes
pinMode(mode1_bryter, INPUT);
pinMode(mode2_bryter, INPUT);
pinMode(acc_x, INPUT);
pinMode(acc_y, INPUT);
}
void loop()
{
//Les data frå knappane/potmeter
int fram = digitalRead(fram_knapp);
int bak = digitalRead(bak_knapp);
int venstre = digitalRead(venstre_knapp);
int hogre = digitalRead(hogre_knapp);
int mode1 = digitalRead(mode1_bryter);
int mode2 = digitalRead(mode2_bryter);
int fart = analogRead(fart_pot);
fart = fart/4;
//lag array for data
byte data[3];
//les data frå akselerometer
int g_sensor_x;
int g_sensor_y;
g_sensor(g_sensor_x, g_sensor_y);
if(mode1 == 0 && mode2 == 0)
{
g_sensor_control(g_sensor_x, g_sensor_y);
}
if (mode1 == 1 && mode2 == 0)
{
//Sett datapakkar igjen
data[0] = 1; //data på plass 0 styrer mode til robot
data[1] = 0; //data plass 1 er ikkje relevant
data[2] = fart; //data[2] styrer fart
//Send datapakka til nRF905
nRF905_setData(data, sizeof(data));
//Send data
while(!nRF905_send());
}
if (mode2 == 1 && mode1 == 0 )
{
//Sett datapakkar
data[1] = 0;
data[0] = 0; //data på plass 0 styrer mode til robot
data[2] = fart;
if(fram == 1)
{
data[1] = 1;
}
else if(bak == 1)
{
data[1] = 2;
}
else if (venstre == 1)
{
data[1] = 3;
}
else if(hogre == 1)
{
data[1] = 4;
}
nRF905_setData(data, sizeof(data));
while(!nRF905_send());
}
}
void g_sensor(int& resultat_x, int& resultat_y)
{
long puls_x = pulseIn(acc_x, HIGH);
long puls_y = pulseIn(acc_y, HIGH);
float duty_x = puls_x/=100;
float duty_y = puls_y/=100;
float konstant = -20.08;
resultat_x = (konstant*duty_x)+996.55;
resultat_y = (konstant*duty_y)+996.55;
}
void g_sensor_control(int g_sensor_x, int g_sensor_y)
{
//Lag array
byte data[3];
if(g_sensor_x > 0 && g_sensor_x > -g_sensor_y && g_sensor_x > g_sensor_y)
{
data[0] = 0;
data[1] = 1;
data[2] = g_sensor_x;
}
if(g_sensor_x < 0 && -g_sensor_x > -g_sensor_y && -g_sensor_x > g_sensor_y)
{
data[0] = 0;
data[1] = 2;
data[2] = -g_sensor_x;
}
if(g_sensor_y >0 && g_sensor_y > -g_sensor_x && g_sensor_y > g_sensor_x)
{
data[0] = 0;
data[1] = 3;
data[2] = g_sensor_y;
}
if(g_sensor_y < 0 && -g_sensor_y > -g_sensor_x && -g_sensor_y > g_sensor_x)
{
data[0] = 0;
data[1] = 4;
data[2] = -g_sensor_y;
}
//send data
nRF905_setData(data, sizeof(data));
while(!nRF905_send());
}
Robot
Kode
//Legg til bibliotek
#include <SoftPWM.h>
#include <SoftPWM_timer.h>
#include <Servo.h>
#include <nRF905.h>
#include <nRF905_config.h>
#include <nRF905_defs.h>
#include <nRF905_types.h>
#include <SPI.h>
//Lag ein servo kalla myservo
Servo myservo;
//Deklarer konstantar for I/O-pins
const int v_b = A0;
const int h_f = A1;
const int v_f = A2;
const int h_b = A3;
const int trig = A4;
const int echo = A5;
//Lag RX/TX-addresser
#define RXADDR 0x99887766
#define TXADDR 0x44332211
void setup()
{
//Sett pinmodes for motor
pinMode(v_b, OUTPUT);
pinMode(h_f, OUTPUT);
pinMode(v_f, OUTPUT);
pinMode(h_b, OUTPUT);
pinMode(trig, OUTPUT);
pinMode(echo, INPUT);
//Velg pin for servo
myservo.attach(6);
//Sett servo i rett posisjon
myservo.write(100);
//Starte nRF905
nRF905_init();
//Sette RX-addresse
nRF905_setRXAddress(RXADDR);
//Sette i receive-modus
nRF905_receive();
//Start softPWM
SoftPWMBegin();
//Sett softPWM pins låge
SoftPWMSet(v_b, 0);
SoftPWMSet(h_f, 0);
SoftPWMSet(v_f, 0);
SoftPWMSet(v_b, 0);
//Sett fade time(Kor lang tid i ms det tar å gå frå 0-255 og 255-0)
SoftPWMSetFadeTime(ALL, 100, 500);
Serial.begin(9600);
}
void loop()
{
//lage variabel for modus
//int modus;
//Motta modebyte frå sendar
//lage buffer for mottak
byte data[3];
//Mottda data (medan mottaker ikkje får data, køyrer den løkka)
while(!nRF905_getData(data, sizeof(data)));
// Sett mode lik data[0];
//modus = data[0];
// Viss modus == 1, sett i automat
if (data[0] == 1)
{
automat_modus();
}
//Viss modus == 0, sett i RC-Modus
if (data[0] == 0)
{
rc_modus();
}
}
void automat_modus()
{
//Lag variabel for lagring av data frå ping))) sensor
long cm;
//Lag variablar for måling av ping)))-sensor høgre/venstre
long cm_venstre, cm_hogre;
//lag buffer for lagring av data frå sendar
byte data[3];
//Hent data frå sendar
while(!nRF905_getData(data, sizeof(data)));
while(data[0]==1)
{
//Hent data frå nRF905
while(!nRF905_getData(data, sizeof(data)));
//Mål avstand
cm = ping();
while(cm > 20)
{
r_fram(data[2]);
cm = ping();
while(!nRF905_getData(data, sizeof(data)));
}
while (cm < 20)
{
r_bak(data[2]); //Bremse
delay(500);
r_stop(); //Stopp
delay(500);
myservo.write(180); // sjå til venstre
delay(1000);
cm_venstre = ping(); // Mål avstand
myservo.write(0); // Sjå til høgre
delay(1000);
cm_hogre = ping(); // Mål avstand
r_bak(data[2]); //Køyr litt tilbake
delay(500);
r_stop(); //Stopp
delay(500);
myservo.write(90); //Sentrer
delay(1000);
//Bestem retning
if (cm_venstre > cm_hogre)
{
r_venstre(data[2]);
delay(500);
r_stop();
delay(500);
}
else
{
r_hogre(data[2]);
delay(500);
r_stop();
delay(500);
}
while(!nRF905_getData(data, sizeof(data)));
cm = ping();
}
}
}
void rc_modus()
{
//array for lagring av data
byte data[3];
//hent data
while(!nRF905_getData(data, sizeof(data)));
//Køyr løkka medan senderen står i RC-modus
while(data[0]==0)
{
while(!nRF905_getData(data, sizeof(data)));
//Serial.println(data[1]);
//Køyrekommandoar for RC_modus data[1]= retning, data[2]=fart
Serial.println(data[2]);
switch(data[1])
{
case 1:
r_fram(data[2]);
break;
case 2:
r_bak(data[2]);
break;
case 3:
r_venstre(data[2]);
break;
case 4:
r_hogre(data[2]);
break;
case 0:
r_stop();
break;
default:
r_stop();
break;
}
}
}
long ping() // Funksjonen måler avstanden til ein gjenstand vha ultralyd
{
long cm;
long puls;
digitalWrite(trig, LOW);
delayMicroseconds(5);
digitalWrite(trig, HIGH);
delayMicroseconds(10);
digitalWrite(trig, LOW);
puls = pulseIn(echo, HIGH);
cm = (puls*34/1000)/2; //(Periodetid*lydhastigheit/(1000000(konvertering til sekund)/100(for å få cm)/2(lyden skal fram og tilbake)
Serial.println(cm);
return cm;
}
//Køyrefunksjonar for motorane (h,v,f,b)
void r_stop() // Robot stopp
{
SoftPWMSet(v_b, 0);
SoftPWMSet(v_f, 0);
SoftPWMSet(h_b, 0);
SoftPWMSet(h_f, 0);
}
void r_fram(int fart) // Robot fram
{
SoftPWMSet(v_b, 0);
SoftPWMSet(h_f, fart);
SoftPWMSet(h_b, 0);
SoftPWMSet(v_f, fart);
}
void r_bak(int fart ) // Robot bak
{
SoftPWMSet(v_b, fart);
SoftPWMSet(v_f, 0);
SoftPWMSet(h_b, fart);
SoftPWMSet(h_f, 0);
}
void r_hogre(int fart) // robot høgre
{
fart = fart*1.2;
if(fart >= 255){fart=255;}
Serial.println(fart);
SoftPWMSet(v_b, 0);
SoftPWMSet(v_f, fart);
SoftPWMSet(h_b, fart);
SoftPWMSet(h_f, 0);
}
void r_venstre(int fart) // robot venstre
{
fart = fart*1.2;
if(fart >= 255){fart=255;}
Serial.println(fart);
SoftPWMSet(v_b, fart);
SoftPWMSet(v_f, 0);
SoftPWMSet(h_b, 0);
SoftPWMSet(h_f, fart);
}
Det er sikkert mykje som kan forbetrast her. Eg har planar om å legge RC modus og auto inn i eigne funksjonar på TX-delen for å få ein litt "finare" loop.
Det største problemet no er at motorane drar for mykje straum. Noko som fører til problemar med ping-sensoren.
