Im working on this project for uni, pretty much im building this system which balances a 5mm ball bearing on a plate with video feedback from a webcam, so that it can be guided through a maze. The robotic section of the design uses 2 servos to control the angle of the plate in order to make the ball move around. Currently I have a Wii Nunchuck connected to it in order to test the dynamics of the system.
I used the WiiChuckClass here: Arduino Playground - WiiChuckClass
as well as the built in servo class. The actual code is an adapted version of the serial example in the WiiChuckClass. The speed of the system can be changed by changing the #define JUMP.
This video shows the prototype im currently working on.
Here is the code:
#include <Servo.h>
#include <WiiChuck.h>
#include "Wire.h"
//#include "WiiChuckClass.h" //most likely its WiiChuck.h for the rest of us.
#define SPEED_DELAY 0
#define JUMP 5
#define TOL 10
WiiChuck chuck = WiiChuck();
Servo xServo, yServo;
int xPos = 0, yPos = 0;
int xJoy = 0, yJoy = 0;
boolean onX = false, onY = false;
void updateServo();
void setup() {
//nunchuck_init();
Serial.begin(115200);
chuck.begin();
chuck.update();
//chuck.calibrateJoy();
xServo.attach(9);
yServo.attach(10);
}
void loop() {
delay(20);
chuck.update();
xJoy = chuck.readJoyX()/2+90;
Serial.print(xJoy);
Serial.print(", ");
yJoy = chuck.readJoyY()/2+90;
Serial.print(yJoy);
Serial.print(", ");
if (chuck.buttonZ) {
Serial.print("Z");
} else {
Serial.print("-");
}
Serial.print(", ");
//not a function// if (chuck.buttonC()) {
if (chuck.buttonC) {
Serial.print("C");
} else {
Serial.print("-");
}
Serial.println();
updateServo();
}
void updateServo() {
if( (xJoy + TOL) > xPos > (xJoy - TOL) && (onX = false) ){
onX = true;
xPos = xJoy;
} else if ((xJoy) > xPos) {
onX = false;
xPos += JUMP;
} else if ((xJoy) < xPos) {
onX = false;
xPos -= JUMP;
}
xServo.write(xPos);
if( (yJoy+TOL) > yPos > (yJoy - TOL) ){
onY = true;
yPos = yJoy;
} else if ((yJoy) > yPos) {
onY = false;
yPos += JUMP;
} else if ((yJoy) < yPos) {
onY = false;
yPos -= JUMP;
}
yServo.write(yPos);
delay(SPEED_DELAY);
}