Cela fait un moment que je ne me suis pas connecté au forum car la robotique m'avait un peu lâcher mais maintenant elle reviens et en ce moment je suis sur un projet utilisant une manette nunchuck sans fils mais voila tout ne marche pas comme je le souhaiterai
le robot sur le quelle je travaille est constituer de 2 servo et de 1 récepteur pour la manette nunchuck sans fil et j'ai plusieurs problèmes:
Ensuite mon 2eme problème est que quand je veut avancer( soit en utilisant les accéléromètre soit le joystick) le robot ne veut pas avancer je doit lui mètre un delay mais quand je met ce delay quand il recule, il recule pendant 3 sec alors que je ne voit pas pourquoi cela n'est pas logique. Auriez-vous une solution à ce second problème??
Voila tout est dit j’espère que vous pourrez m'aider.
voila mon code :
#include <Servo.h>
int axel =0;
#define AVANT 1
#define ARRIERE 0
#define GAUCHE 2
#define DROITE 3
#define STOP 321
#define SERVOG 1
#define SERVOD 0
#define SHARP 4
#define VMAX 5
Servo servog;
Servo servod;
int vLdr = 5;
#include <Wire.h>
#include <math.h>
#include <utility/twi.h>
#define RADIUS 210
#define numReadings 7
byte cnt;
uint8_t status[6];
byte averageCounter;
int i;
int total;
uint8_t zeroJoyX = 127;
uint8_t zeroJoyY = 134;
int ZEROX = 510;
int ZEROY = 460;
int ZEROZ = 0;
int lastJoyX;
int lastJoyY;
int angles[3];
boolean lastZ, lastC;
byte joyX;
byte joyY;
boolean buttonZ;
boolean buttonC;
const byte NunchuckID[6] = {0x00, 0x00, 0xA4, 0x20, 0x00, 0x00};
byte getID[6] = {0};
byte connection = 0;
byte C;
byte Z;
int aveJX;
int aveJY;
int aveAX;
int aveAY;
int readJX[numReadings];
int readJY[numReadings];
int readAX[numReadings];
int readAY[numReadings];
int ledPin = 13;
int getNeutral(int s)
{
if(s == SERVOG)
return 90;
else
return 90;
}
void handleS(int s, int speed)
{
int tab[6] = {0,1,3,8,10,30};
int pos = getNeutral(s);
int acc;
if (speed < 0)
acc = - tab[abs(speed)];
else
acc = tab[speed];
if(s == SERVOG)
servog.write(pos += acc);
else
servod.write(pos -= acc);
}
void move(int direction)
{
int m1 = 0, m2 = 0;
switch(direction)
{
case DROITE: m1 = 1; m2 = -1; digitalWrite(8, LOW); break;
case GAUCHE: m1 = -1; m2 = 1; digitalWrite(8, LOW); break;
case AVANT: m1 = 1; m2 = 1; digitalWrite(8, LOW); break;
case ARRIERE: m1 = -1; m2 = -1; digitalWrite(8, HIGH); break;
case STOP: m1 = 0; m2 = 0;
}
handleS(SERVOG, m1*VMAX);
handleS(SERVOD, m2*VMAX);
}
void sharp()
{
if (Z == 1 ){
axel = 0;
}
if (C == 1){
axel = 1;
}
if (Z == 1 & axel == 1 ){
axel = 0;
}
if (C == 1 & axel == 0 ){
axel = 1;
}
if(axel == 0){
if (aveAY < 70){
move(AVANT);
delay(1500);
}
if (aveAY > 380)
{
move(ARRIERE);
}
else {
move(STOP);
}
}
if(axel ==1 ){
if ( aveJY < -80){
move(AVANT);
delay(500);
}
if (aveJY > 80)
{
move(ARRIERE);
}
else {
move(STOP);
}
}
}
void tourne(){
if(axel == 0){
if (aveAX < -280){
move(DROITE);
delay(1);
}
if (aveAX > 70)
{
move(GAUCHE);
}
}
if(axel ==1 ){
if (aveJX < -80){
move(DROITE);
delay(1);
}
if (aveJX > 80)
{
move(GAUCHE);
}
}
}
void setup()
{
servog.attach(10);
servod.attach(9);
pinMode(ledPin, OUTPUT);
Serial.begin(9600);
Serial.println("Start");
Wire.begin();
#define TWI_FREQ_NUNCHUCK 400000L
TWBR = ((CPU_FREQ / TWI_FREQ_NUNCHUCK) - 16) / 2;
cnt = 0;
averageCounter = 0;
while(connection == 0)
{
Wire.beginTransmission (0x52);
Wire.send (0xF0);
Wire.send (0x55);
if(Wire.endTransmission () == 0)
{
Serial.println("0x55 data send success");
Wire.beginTransmission (0x52);
Wire.send (0xFB);
Wire.send (0x00);
if(Wire.endTransmission () == 0) Serial.println("0x00 data send success");
else Serial.println("0x00 data send failed");
do
{
Wire.beginTransmission (0x52);
Wire.send (0xFA);
delay(1);
Serial.println(Wire.endTransmission(),DEC);
}
while(Wire.endTransmission() != 0);
Wire.requestFrom(0x52, 6);
if(Wire.available() == 0) connection = 2;
else
{
byte i=0;
while(Wire.available())
{
getID[i] = Wire.receive();
i++;
}
for(byte j=0; j<6; j++)
{
if(getID[j] == NunchuckID[j])
{
if(j==0) connection = 1;
else if(j>0 && connection == 1) connection = 1;
else connection = 0;
}
else connection = 0;
Serial.println(connection,DEC);
}
}
}
else Serial.println("0x55 data send failed");
}
if(connection == 1)
{
ZEROX = 610;
ZEROY = 290;
ZEROZ = 0;
zeroJoyX = 128;
zeroJoyY = 124;
for(byte j=0;j<6;j++)
{
Serial.print(getID[j], HEX);
Serial.print(" ");
}
delay(3000);
}
update();
for (i = 0; i<3;i++)
angles[i] = 0;
for(int j=0;j<=6;j++)
readJX[j] = readJY[j] = readAX[j] = readAY[j] = 0;
}
void calibrateJoy() {
zeroJoyX = joyX;
zeroJoyY = joyY;
}
void update() {
Wire.requestFrom (0x52, 6);
while (Wire.available ()) {
status[cnt] = _nunchuk_decode_byte (Wire.receive());
cnt++;
}
if (cnt > 5) {
lastZ = buttonZ;
lastC = buttonC;
lastJoyX = readJoyX();
lastJoyY = readJoyY();
cnt = 0;
joyX = (status[0]);
joyY = (status[1]);
for (i = 0; i < 3; i++)
angles[i] = (status[i+2] << 2) + ((status[5] & (B00000011 << ((i+1)*2) ) >> ((i+1)*2)));
buttonZ = !( status[5] & B00000001);
buttonC = !((status[5] & B00000010) >> 1);
_send_zero();
}
}
float readAccelX() {
return (float)angles[0] - ZEROX;
}
float readAccelY() {
return (float)angles[1] - ZEROY;
}
float readAccelZ() {
return (float)angles[2] - ZEROZ;
}
boolean zPressed() {
return (buttonZ && ! lastZ);
}
boolean cPressed() {
return (buttonC && ! lastC);
}
boolean buttC() {
return buttonC;
}
boolean buttZ() {
return buttonZ;
}
boolean rightJoy(int thresh=60) {
return (readJoyX() > thresh and lastJoyX <= thresh);
}
boolean leftJoy(int thresh=60) {
return (readJoyX() < -thresh and lastJoyX >= -thresh);
}
int readJoyX() {
return (int) joyX - zeroJoyX;
}
int readJoyY() {
return (int)joyY - zeroJoyY;
}
int readRoll() {
return (int)(atan2(readAccelX(),readAccelZ())/ M_PI * 180.0);
}
int readPitch() {
return (int) (acos(readAccelY()/RADIUS)/ M_PI * 180.0);
}
byte _nunchuk_decode_byte (byte x)
{
return x;
}
void _send_zero()
{
Wire.beginTransmission (0x52);
Wire.send (0x00);
Wire.endTransmission ();
}
void loop()
{
sharp();
tourne();
update();
SGFilter();
if(Z == 1){
digitalWrite(ledPin, HIGH);
}
if(C == 1){
digitalWrite(ledPin, LOW);
}
Serial.print(aveJX, DEC);
Serial.print("\t");
Serial.print(aveJY, DEC);
Serial.print("\t");
Serial.print(aveAX, DEC);
Serial.print("\t");
Serial.print(aveAY, DEC);
Serial.print("\t");
Serial.print( Z, DEC );
Serial.print("\t");
Serial.print( C, DEC );
Serial.print("\t");
Serial.print( connection, DEC );
Serial.print("\r\n");
}
void SGFilter()
{
for(int index = 0; index<6; index++) {
readJX[index] = readJX[index+1];
readJY[index] = readJY[index+1];
readAX[index] = readAX[index+1];
readAY[index] = readAY[index+1];
}
readJX[6] = readJoyX();
readJY[6] = readJoyY();
readAX[6] = (int)readAccelX();
readAY[6] = (int)readAccelY();
if(buttZ() == true) Z=1;
else Z=0;
if(buttC() == true) C=1;
else C=0;
aveJX = ((-2*readJX[0]) + (3*readJX[1]) + (6*readJX[2]) + (7*readJX[3]) + (6*readJX[4]) + (3*readJX[5]) - (2*readJX[6]))/21;
aveJY = ((-2*readJY[0]) + (3*readJY[1]) + (6*readJY[2]) + (7*readJY[3]) + (6*readJY[4]) + (3*readJY[5]) - (2*readJY[6]))/21;
aveAX = ((-2*readAX[0]) + (3*readAX[1]) + (6*readAX[2]) + (7*readAX[3]) + (6*readAX[4]) + (3*readAX[5]) - (2*readAX[6]))/21;
aveAY = ((-2*readAY[0]) + (3*readAY[1]) + (6*readAY[2]) + (7*readAY[3]) + (6*readAY[4]) + (3*readAY[5]) - (2*readAY[6]))/21;
}
merci d'avance
axe19
