new code
I found this code on instructables by oomlout for the Serb robot. I am going to see if I can figure out how to take this and make two sketches. send and receive.
/*****************************************************************
* Arduino Controlled Servo Robot (SERB) - Wii Nunchuck Control *
* For more details visit: http://www.oomlout.com/serb *
* *
* Behaviour: Uses a Wii Nunchuck to control the movement of *
* your SERB. Use the joystick to move the robot *
* forward and backwards, left and right. *
* Hold the Z button to use the acelerometers, *
* tilt the nunchuck forward to go forward, backwards *
* to go in reverse and left and right, to go left *
* and right. *
* *
* Wiring: --Arduino Controlled Servo Robot-- *
* Right Servo Signal - pin 9 *
* Left Servo Signal - pin 10 *
* --Wii Nunchuck-- *
* + - white - ground - negative *
* - - red - 3.3+v - 5 volts will work *
* d - green - data - Analog 4 *
* c - yellow - clock - Analog 5 *
* *
* License: This work is licenced under the Creative Commons *
* Attribution-Share Alike 3.0 Unported License. To *
* view a copy of this licence, visit *
* http://creativecommons.org/licenses/by-sa/3.0/ *
* or send a letter to Creative Commons, 171 Second *
* Street, Suite 300, San Francisco, California 94105, *
* USA. *
* *
*************************************************************/
//START OF NUNCHUCK PREAMBLE -
//For more in depth information please visit the original source
//of this code http://www.windmeadow.com/node/42
//----------------------------------------------------------------
#include <Wire.h>
#include <string.h>
#undef int
#include <stdio.h>
uint8_t outbuf[6]; // array to store arduino output
int cnt = 0; // counter used for nunchuck
// comunication
int ledPin = 13;
int nunchuckValues[] = {
0,0,0,0,0,0,0,0,0,0}; //An array to store
//the nunchuck values
/
* The index of each value within the nunchuckValues[] array
* ie. XSTICK value is stored at nunchuckValues[XSTICK] (0)
/
#define XSTICK 0 //The joystick values
#define YSTICK 1
#define XAXIS 2 //The three accelerometer values
#define YAXIS 3
#define ZAXIS 4
#define ZBUTTON 5 //Front button values (0 when pressed)
#define CBUTTON 6
#define XAXISDELTA 7 //Change in accelerometer data from last read;
#define YAXISDELTA 8
#define ZAXISDELTA 9
/
* Nunchuck G calculating Constants
* Each accelerometer returns a value between 0 and 1024 (10 bits)
* resting at about 512 in the middle. To make this more usable in
* our code we can scale this value to number of Gs. This requires
* a number of constants for details on what these constants mean
* and how to callculate them for your nunchuck visit.
* – http://www.wiili.org/index.php/Motion_analysis –
* The constants to follow are named identical to the explanation
* in that page. (some are unused in the code and could be deleted)
* Zero Points
* <math>x_0 = (x_1 + x_2) / 2,</math>
* <math>y_0 = (y_1 + y_3) / 2,</math>
* <math>z_0 = (z_2 + z_3) / 2,</math>
* One G points
* <math>x = \frac{x_{raw} - x_0}{x_3 - x_0}</math>
* <math>y = \frac{y_{raw} - y_0}{y_2 - y_0}</math>
* <math>z = \frac{z_{raw} - z_0}{z_1 - z_0}</math>
/
#define X0 500
#define X1 500
#define X2 500
#define X3 711
#define Y0 465
#define Y1 481
#define Y2 621
#define Y3 449
#define Z0 578
#define Z1 785
#define Z2 575
#define Z3 582
//END OF NUNCHUCK PREAMBLE
//----------------------------------------------------------------
//----------------------------------------------------------------
// START OF ARDUINO CONTROLLED SERVO ROBOT (SERB) PREAMBLE
#include <Servo.h>
#define LEFTSERVOPIN 10 //The pin the left Servo is connected to
#define RIGHTSERVOPIN 9 //The pin the right Servo is connected to
#define MAXSPEED 10 //due to the way continuous rotation servos
//work maximum speed is reached at a much
//lower value than 90 (this value will change
//depending on your servos) (for Parallax servos)
//20 will give you full range 10 makes it very
//controllable but a little slow
Servo leftServo;
Servo rightServo;
int leftSpeed; //sets the speed of the robot (left servos)
//a percentage between -MAXSPEED and MAXSPEED
int rightSpeed; //sets the speed of the robot (both servos)
//a percentage between -MAXSPEED and MAXSPEED
int speed = 100; //used for simple control (goForward etc.)
//a percentage between 0 and MAXSPEED
// END OF ARDUINO CONTROLLED SERVO ROBOT (SERB) PREAMBLE
//----------------------------------------------------------------
// THIS PROGRAMS PREAMBLE
//----------------------------------------------------------------
long lastPrint; //a long variable to store the time the
//wiimote state was last printed
#define PRINTINTERVAL 1000 //the number of milliseconds between
//outputting the nunchuck state over the
//usb port
#define DEADBAND 20 //A percentage away from center that is
//interpretted as still being zero
//this accounts for joysticks or
//accelerometers that aren’t quite centered
// END OF THIS PROGRAMS PREAMBLE
//----------------------------------------------------------------
void setup(){
beginSerial (9600); //Starts the serial port
nunchuck_init (); //send the nunchuck initilization handshake
serbSetup(); //adds the servos and prepares all
//SERB related variables
lastPrint = millis(); //loads the current time as the last time
//nunchuck data was sent over the usb port
}
/
* The main loop
* Reads the nunchuck data, then if the Z Button is pressed calls a
* routine to move based on current accelerometer values otherwise
* it will call a routine to use the current joystick values
* finally if PRINTINTERVAL time has elapsed the current nunchuck
* values are sent over the USB port.
/
void loop(){
readNunchuck(); //Reads the current state of the
//nunchucks buttons and accelerometers
if(!getNunValue(ZBUTTON)){ //If the Z Button is pressed
moveWiiAcelerometer(); //moves the robot depending on the
//nunchucks acceleration values
}
else{ //If the Z Button is not pressed
moveWiiJoystick(); //moves the robot depending on the
//nunchucks joystick values
}
if((millis() - lastPrint) > PRINTINTERVAL){ //If a PRINTINTERVAL
//milliseconds has
//passed since last
//printing nunchuck
//values print them
printData(); //print nunchuck values
Serial.println(); //add an enter
lastPrint = millis(); //store current time as lastPrint
}
}
/
* Moves the robot based on the accelerometer values. If tilted one
* G forward the full speed forward, half a G forward then half
* speed forward etc.
/
void moveWiiAcelerometer(){
moveDifferential(getYGs() * (float)100,getXGs()(float)100);
//Scales the -1 to 1 G value from the accelerometers to -100 to
//100 percent for the moveDifferential routine
//Using the y axis for speed and x axis for direction
}
/
* Moves the robot based on the joystick values. If pushed fully forward
* then full speed forward, half a forward then half
* speed forward etc.
/
void moveWiiJoystick(){
moveDifferential(map(getNunValue(YSTICK),30,220,-100,100),map(getNunValue(XSTICK),30,220,-100,100));
//Scales the Y axis stick from 30 to 220 to -100 to 100 for the
//moveDifferential command
//Using the y axis for speed and x axis for direction
}
/
* takes in a speed value between 100 and -100 and a direction value
* from -100 to 100.
* example out comes. speed 100 direction 0 = full forward
* speed 0 direction -100 = full speed left
* speed 0 direction 100 = full speed right
/
void moveDifferential(int speed1, int direction1){
speed1 = deadBandFilter(speed1); //if speed is within DEADBAND percent
// of zero then treat as zero
direction1 = deadBandFilter(direction1); //if direction is within
//DEADBAND percent of
//zero then treat as zero
setSpeedLeft(speed1 + direction1); //left speed is speed one plus
//the direction value
setSpeedRight(speed1 - direction1); //right speed is speed minus the
//direction value
}
/
* Adds a dead spot to control values. ie if a value is within DEADBAND
* percent of zero the treat as zero, if above DEADBAND percent of zero
* then scale the value to be between 0 and 100 when input value is
* DEADBAND to 100
/
int deadBandFilter(int value){
if(value > -DEADBAND && value < DEADBAND){ //if value is within
value = 0; //DEADBAND percent of zero
} //then make it zero
else{
if(value > 0){
value = value - DEADBAND * 100 / (100-DEADBAND); //scale
}
else{
value = value + DEADBAND * 100 / (100-DEADBAND); //scale
}
}
return value;
}
//START OF NUNCHUCK ROUTINES
//-------------------------------------------------------------------------------------------------------
/
* Returns the last read value for a particular input index
* constants are at the head of this program
* example call: getNunValue(YAXIS)
/
int getNunValue(int valueIndex){
return nunchuckValues[valueIndex];
}
/
* Calculates and returns the xAxis acceleration in Gs
/
float getXGs(){
return ((float)getNunValue(XAXIS) - X0) / (X3 - X0);
//using math from – http://www.wiili.org/index.php/Motion_analysis –
}
/
* Calculates and returns the yAxis acceleration in Gs
/
float getYGs(){
return ((float)getNunValue(YAXIS) - Y0) / (Y2 - Y0);
//using math from – http://www.wiili.org/index.php/Motion_analysis –
}
/
* Calculates and returns the zAxis acceleration in Gs
/
float getZGs(){
return ((float)getNunValue(YAXIS) - Z0) / (Z1 - Z0);
//using math from – http://www.wiili.org/index.php/Motion_analysis –
}
//START OF NUNCHUCK Reading CODE - For more in depth information
//please visit the original source
//of this code http://www.windmeadow.com/node/42
//---------------------------------------------------------------
void readNunchuck(){
Wire.requestFrom (0x52, 6); // request data from nunchuck
while (Wire.available ()) {
outbuf[cnt] = nunchuk_decode_byte (Wire.receive ()); // receive byte as an integer
digitalWrite (ledPin, HIGH); // sets the LED on
cnt++;
}
// If we recieved the 6 bytes, then go print them
if (cnt >= 5) {
nunchuckValues[XSTICK] = outbuf[0];
nunchuckValues[YSTICK] = outbuf[1];
int tempNun_xAxis = outbuf[2] * 2 * 2;
int tempNun_yAxis = outbuf[3] * 2 * 2;
int tempNun_zAxis = outbuf[4] * 2 * 2;
nunchuckValues[ZBUTTON] = 0;
nunchuckValues[CBUTTON] = 0;
// byte outbuf[5] contains bits for z and c buttons
// it also contains the least significant bits for the accelerometer data
// so we have to check each bit of byte outbuf[5]
if ((outbuf[5] >> 0) & 1) {
nunchuckValues[ZBUTTON] = 1;
} //checking if Z button is pressed (0=pressed 1=unpressed)
if ((outbuf[5] >> 1) & 1) {
nunchuckValues[CBUTTON] = 1;
} //checking if C button is pressed (0=pressed 1=unpressed)
if ((outbuf[5] >> 2) & 1) {
tempNun_xAxis += 2;
} //adding second least significant bit to x_axis
if ((outbuf[5] >> 3) & 1) {
tempNun_xAxis += 1;
} //adding least significant bit to x_axis
if ((outbuf[5] >> 4) & 1) {
tempNun_yAxis += 2;
} //adding second least significant bit to y_axis
if ((outbuf[5] >> 5) & 1) {
tempNun_yAxis += 1;
} //adding least significant bit to x_axis
if ((outbuf[5] >> 6) & 1) {
tempNun_zAxis += 2;
} //adding second least significant bit to z_axis
if ((outbuf[5] >> 7) & 1) {
tempNun_zAxis += 1;
} ////adding least significant bit to x_axis
nunchuckValues[XAXISDELTA] = tempNun_xAxis - nunchuckValues[XAXIS];
nunchuckValues[XAXIS] = tempNun_xAxis;
nunchuckValues[YAXISDELTA] = tempNun_yAxis - nunchuckValues[YAXIS];
nunchuckValues[YAXIS] = tempNun_yAxis;
nunchuckValues[ZAXISDELTA] = tempNun_zAxis - nunchuckValues[ZAXIS];
nunchuckValues[ZAXIS] = tempNun_zAxis;
}
cnt = 0;
send_zero (); // send the request for next bytes
}
void nunchuck_init (){
Wire.begin (); // join i2c bus with address 0x52
Wire.beginTransmission (0x52); // transmit to device 0x52
Wire.send (0x40); // sends memory address
Wire.send (0x00); // sends sent a zero.
Wire.endTransmission (); // stop transmitting
}
void send_zero () {
Wire.beginTransmission (0x52); // transmit to device 0x52
Wire.send (0x00); // sends one byte
Wire.endTransmission (); // stop transmitting
}
// Encode data to format that most wiimote drivers except
// only needed if you use one of the regular wiimote drivers
char nunchuk_decode_byte (char x) {
x = (x ^ 0x17) + 0x17;
return x;
}
//END OF NUNCHUCK CODE - For more in depth information please visit
//the original source of this code http://www.windmeadow.com/node/42
//---------------------------------------------------------------
//------------------------------------------------------------------------
//START OF ARDUINO CONTROLLED SERVO ROBOT (SERB) ROUTINES
/
* sets up your arduino to address your SERB using the included
* routines
/
void serbSetup(){
setSpeed(speed);
pinMode(LEFTSERVOPIN, OUTPUT); //sets the left servo signal pin
//to output
pinMode(RIGHTSERVOPIN, OUTPUT); //sets the right servo signal pin
//to output
leftServo.attach(LEFTSERVOPIN); //attaches left servo
rightServo.attach(RIGHTSERVOPIN); //attaches right servo
goStop();
}
/
* sets the speed of the robot between 0-(stopped) and 100-(full speed)
* NOTE: speed will not change the current speed you must change speed
* then call one of the go methods before changes occur.
/
void setSpeed(int newSpeed){
if(newSpeed >= 100) {
newSpeed = 100;
} //if speed is greater than 100
//make it 100
if(newSpeed <= 0) {
newSpeed = 0;
} //if speed is less than 0 make
//it 0
speed = newSpeed * MAXSPEED / 100; //scales the speed to be
//between 0 and MAXSPEED
}
/
* sets the speed of the robots rightServo between -100-(reversed) and 100-(forward)
* NOTE: calls to this routine will take effect imediatly
/
void setSpeedRight(int newSpeed){
if(newSpeed >= 100) {
newSpeed = 100;
} //if speed is greater than 100
//make it 100
if(newSpeed <= -100) {
newSpeed = -100;
} //if speed is less than -100 make
//it -100
rightSpeed = newSpeed * MAXSPEED / 100; //scales the speed to be
//between -MAXSPEED and MAXSPEED
rightServo.write(90 - rightSpeed); //sends the new value to the servo
}
/
* sets the speed of the robots leftServo between -100-(reversed) and 100-(forward)
* NOTE: calls to this routine will take effect immediatly
/
void setSpeedLeft(int newSpeed){
if(newSpeed >= 100) {newSpeed = 100;} //if speed is greater than 100
//make it 100
if(newSpeed <= -100){newSpeed = -100;}//if speed is less than -100
//make it -100
leftSpeed = newSpeed * MAXSPEED / 100;//scales the speed to be
//between -MAXSPEED and MAXSPEED
leftServo.write(90 + leftSpeed); //sends the new value to the servo
}
/
* sends the robot forwards
/
void goForward(){
leftServo.write(90 + speed);
rightServo.write(90 - speed);
}
/
* sends the robot backwards
/
void goBackward(){
leftServo.write(90 - speed);
rightServo.write(90 + speed);
}
/
* sends the robot right
/
void goRight(){
leftServo.write(90 + speed);
rightServo.write(90 + speed);
}
/
* sends the robot left
/
void goLeft(){
leftServo.write(90 - speed);
rightServo.write(90 - speed);
}
/
* stops the robot
*/
void goStop(){
leftServo.write(90);
rightServo.write(90);
}
//END OF ARDUINO CONTROLLED SERVO ROBOT (SERB) ROUTINES
//---------------------------------------------------------------------------
//START OF PRINT ROUTINES (can delete if not using)
//---------------------------------------------------------------
//Prints the Nunchucks last read data (must call NUN_readNunchuck();
//before calling
void printData(){
Serial.print(“XJoy= “); Serial.print (getNunValue(XSTICK), DEC); Serial.print (”\t”);
Serial.print(“YJoy= “); Serial.print (getNunValue(YSTICK), DEC); Serial.print (”\t”);
Serial.print(“XGs= “); Serial.print (getXGs() * 1000, DEC); Serial.print (”\t”);
Serial.print(“YGs= “); Serial.print (getYGs() * 1000, DEC); Serial.print (”\t”);
Serial.print(“ZGs= “); Serial.print (getZGs() * 1000, DEC); Serial.print (”\t”);
Serial.print(“ZBut= “); Serial.print (getNunValue(ZBUTTON), DEC);Serial.print (”\t”);
Serial.print(“YBut= “); Serial.print (getNunValue(CBUTTON), DEC);Serial.print (”\t”);
}
//END OF PRINT ROUTINES
//--------------------------------------------------------------------
