Arduino labyrint (4 / 4 steg)

Steg 4: Arduino kod

#include / / Arduino bibliotek Wire.h
#include / / Arduino bibliotek string.h
#include / / Arduino bibliotek Stdio.h
#include / / Arduino bibliotek Servo.h

Servo myservo; skapa objekt för att styra trigger servo servo

int pos = 90. variabel att lagra trigger servo position

uint8_t outbuf [6].

int cnt = 0;
int ledPin = 13.
int ledPin12 = 12;
int Z_trig;

int servoPin = 7.
int servoPin2 = 6;

int pulseWidth = 0;
int pulseWidth2 = 0;

lång lastPulse = 0;
lång lastPulse2 = 0;

int z_button = 0;
int c_button = 0;

int refreshTime = 20; tweaked för Hi-tech servon

int minPulse = 300; tweaked för Hi-tech 3-11 servon
int minPulse2 = 300; tweaked för Hi-tech 3-11 servon
int maxPulse = 2500; maximal servo position
int maxPuls2e = 2500; maximal servo position

int dtime = 10;

#define pwbuffsize 10
lång pwbuff [pwbuffsize];
lång pwbuffpos = 0;
lång pwbuff2 [pwbuffsize];
lång pwbuffpos2 = 0;

void setup()
{
myservo.attach(8);

Serial.BEGIN (9600);
Wire.BEGIN ();
nunchuck_init ();
pinMode (servoPin, produktionen);
pinMode (servoPin2, produktionen);

pulseWidth = minPulse;
pulseWidth2 = minPulse2;

Serial.Print ("färdiga setup\n");
}

void nunchuck_init() / / Wii Nun-chuck handskakning
{
Wire.beginTransmission (0x52);
Wire.send (0x40);
Wire.send (0x00);
Wire.endTransmission ();
}

void send_zero() / / Wii Nun-chuck handskakning
{
Wire.beginTransmission (0x52);
Wire.send (0x00);
Wire.endTransmission ();
}

int t = 0;

void loop()
{
pinMode (ledPin12, produktionen); uppsättningar pin 12 som en utgång så att nödsänkningssolenoiden kan kontrolleras av Wii Nunchuck Z-knappen
t ++;
äntligen = millis();

om (t == 1) {

t = 0;

Wire.requestFrom (0x52, 6);

medan (Wire.available ()) {
outbuf [cnt] = nunchuk_decode_byte (Wire.receive ());
digitalWrite (ledPin, hög);
CNT ++;
}

om (cnt > = 5) {

int z_button = 0; initierar Z till noll
int c_button = 0; initierar C till noll

om ((outbuf [5] >> 0) & 1) / / undersöker Byte fem från the Wii-Nunchuck för position i Z-knappen
z_button = 1; uppsättningar Z 1
om ((outbuf [5] >> 1) & 1) / / undersöker Byte fem från the Wii-Nunchuck för position i Z-knappen
c_button = 1; anger C till 1

växel (c_button) {/ / mål uttalande för C knappen tillstånd
fall 1:

muovi (outbuf [1] / 2 + 0x3E, outbuf [0] / 2 + 0x3E); Aktiverar Accelromiter deadlinekontroll för Wii Nunchuck
bryta;
fall 0:
muovi (outbuf [3], outbuf [2]); underslag Styrspaksreglage för Wii nunchuck
bryta;
}

växel (z_button) {/ / mål uttalande för Z-knappen stat
fall 1: {
digitalWrite (ledPin12, låg); standard magnetventil är avstängd
Z_trig = 0;}
bryta;
fall 0: {
om (Z_trig == 0) {
digitalWrite (ledPin12, hög); Aktiverar nödsänkningssolenoiden
fördröjning (20);
digitalWrite (ledPin12, låg); deaktiverar nödsänkningssolenoiden om Z-knappen hålls för att skydda från att bryta
Z_trig = 1 ;}}

bryta;
}

}

CNT = 0;
send_zero();

}

updateServo();

Delay(dtime);
}

void updateServo()
{

om (millis() - lastPulse > = refreshTime) {

digitalWrite (servoPin, hög);
delayMicroseconds(pulseWidth);
digitalWrite (servoPin, låg);

digitalWrite (servoPin2, hög);
delayMicroseconds(pulseWidth2);
digitalWrite (servoPin2, låg);

lastPulse = millis();
}
}

int jag = 0;

char nunchuk_decode_byte (char x)
{
x = (x ^ 0x17) + 0x17;
återvändande x;
}

void muovi (uint8_t x, uint8_t y) {
flyta tilt = (700 - x * 2 * 2);
flyta tilt2 = (0x7E - y + 0x7E) * 2 * 2.

lutning = (lutning);
pulseWidth = (luta * 5) + minPulse;

Tilt2 = (tilt2-288);
pulseWidth2 = (tilt2 * 5) + minPulse2;

pwbuff [pwbuffpos] = pulseWidth;
pwbuff2 [pwbuffpos2] = pulseWidth2;

om (++ pwbuffpos == pwbuffsize) pwbuffpos = 0;
om (++ pwbuffpos2 == pwbuffsize) pwbuffpos2 = 0;

pulseWidth = 0;
pulseWidth2 = 0;

för (int p = 0, p pulseWidth += pwbuff [p];
pulseWidth2 += pwbuff2 [p];
}

pulseWidth = pwbuffsize;
pulseWidth2 = pwbuffsize;
}