Hi guys,
Im new to arduino, and I want to control my servos/motors using the wii motion plus gyroscope.
Unfortunately the servo is not moving at all.
Any ideas how I can get that fixed?
thanks!
setup looks like this:
and that's the code I'm using:
//Trapezoid or Runge-kutta 4th order integration program of wm+
#include <Streaming.h>
#include <Wire.h>
#include <Servo.h>
Servo yawServo;
Servo pitchServo;
int yawServoVal;
int pitchServoVal;
#define steps_per_deg_slow 20
#define steps_per_deg_fast 4
byte data[6]; //six data bytes
int yaw, pitch, roll; //three axes
int yaw0, pitch0, roll0; //calibration zeroes
int time, last_time;
float delta_t;
int last_yaw[3], last_pitch[3], last_roll[3];
float yaw_deg, pitch_deg, roll_deg;
int yaw_deg2, pitch_deg2, roll_deg2;
int startTag=0xDEAD;
int accel_x_axis, accel_y_axis, accel_z_axis;
void wmpOn(){
Wire.beginTransmission(0x53); //WM+ starts out deactivated at address 0x53
Wire.send(0xfe); //send 0x04 to address 0xFE to activate WM+
Wire.send(0x04);
Wire.endTransmission(); //WM+ jumps to address 0x52 and is now active
}
void wmpSendZero(){
Wire.beginTransmission(0x52); //now at address 0x52
Wire.send(0x00); //send zero to signal we want info
Wire.endTransmission();
}
void calibrateZeroes(){
for (int i=0;i<10;i++){
wmpSendZero();
Wire.requestFrom(0x52,6);
for (int i=0;i<6;i++){
data[i]=Wire.receive();
}
yaw0+=(((data[3]>>2)<<8)+data[0])/10; //average 10 readings for each zero
pitch0+=(((data[4]>>2)<<8)+data[1])/10;
roll0+=(((data[5]>>2)<<8)+data[2])/10;
}
Serial.print("Yaw0:");
Serial.print(yaw0);
Serial.print(" Pitch0:");
Serial.print(pitch0);
Serial.print(" Roll0:");
Serial.println(roll0);
}
void receiveData(){
wmpSendZero(); //send zero before each request (same as nunchuck)
Wire.requestFrom(0x52,6); //request the six bytes from the WM+
for (int i=0;i<6;i++){
data[i]=Wire.receive();
}
if(bitRead(data[3], 1)==1) yaw=(((data[3]>>2)<<8)+data[0]-yaw0)/steps_per_deg_slow; //see http://wiibrew.org/wiki/Wiimote/Extension_Controllers#Wii_Motion_Plus
else yaw=(((data[3]>>2)<<8)+data[0]-yaw0)/steps_per_deg_fast;
if(bitRead(data[3], 0)==1) pitch=(((data[4]>>2)<<8)+data[1]-pitch0)/steps_per_deg_slow; //for info on what each byte represents
else pitch=(((data[4]>>2)<<8)+data[1]-pitch0)/steps_per_deg_fast;
if(bitRead(data[4], 1)==1) roll=(((data[5]>>2)<<8)+data[2]-roll0)/steps_per_deg_slow;
else roll=(((data[5]>>2)<<8)+data[2]-roll0)/steps_per_deg_fast;
}
float trapIntegrate(int y2, int y1, float deltax){
float area=0;
area=(y2+y1)/2*deltax/1000;
return area;
}
float rk4Integrate(int y4, int y3, int y2, int y1, float deltax){
float area=0;
area=((y4+2*y3+2*y2+y1)/6)*deltax/1000;
return area;
}
void setup()
{
Serial.begin(115200);
Serial.println("WM+ Tuning People Bot Control");
Wire.begin();
wmpOn(); //turn WM+ on
calibrateZeroes(); //calibrate zeroes
delay(1000);
yawServo.attach(2);
pitchServo.attach(3);
pinMode(4, INPUT);
}
void loop(){
receiveData(); //receive data and calculate yaw pitch and roll
time=millis();
delta_t=(time-last_time);
/* Runge-kutta 4th Order Integration */
yaw_deg+=rk4Integrate(yaw, last_yaw[0], last_yaw[1], last_yaw[2], delta_t);
pitch_deg+=rk4Integrate(pitch, last_pitch[0], last_pitch[1], last_pitch[2], delta_t);
roll_deg+=rk4Integrate(roll, last_roll[0], last_roll[1], last_roll[2], delta_t);
last_yaw[2]=last_yaw[1];
last_pitch[2]=last_pitch[1];
last_roll[2]=last_roll[1];
last_yaw[1]=last_yaw[0];
last_pitch[1]=last_pitch[0];
last_roll[1]=last_roll[0];
last_yaw[0]=yaw;
last_pitch[0]=pitch;
last_roll[0]=roll;
last_time=time;
/* Runge-kutta 4th Order Integration */
if (digitalRead(4) == 1)
{
last_yaw[2]=0;
last_pitch[2]=0;
last_roll[2]=0;
last_yaw[1]=0;
last_pitch[1]=0;
last_roll[1]=0;
last_yaw[0]=0;
last_pitch[0]=0;
last_roll[0]=0;
yaw = 0;
pitch = 0;
roll = 0;
yaw_deg = 0;
pitch_deg = 0;
roll_deg = 0;
}
yawServoVal = map(yaw_deg, -90, 90, 0, 179);
if (yawServoVal < 0){yawServoVal = 0;};
if (yawServoVal > 179){yawServoVal = 179;};
yawServo.write(yawServoVal);
pitchServoVal = map(pitch_deg, -90, 90, 0, 179);
if (pitchServoVal < 60){pitchServoVal = 60;};
if (pitchServoVal > 179){pitchServoVal = 179;};
pitchServo.write(pitchServoVal);
Serial<<yaw_deg<<"\t"<<pitch_deg<<"\t"<<roll_deg<<"\n";
delay(10);
}
Where's your test sketch to prove the servo works?
What does your debug output look like?
Does it look like you're passing the right sort of values to the servos?
How about the external power supply, are you running it with the USB-power only?
Kari
the servo is working. I now tested it on an 12V battery, still not working 
. I now tested it on an 12V battery
What sort of servo is it?
Most R/C servos are 6V.
Do the debug prints look sensible?
i.e. do they gives servo angles in the right sort of range?
I've got to say, R-K wouldn't be my first attempt at an Arduino programming project
Thanks guys, it's a 4.8V servo.
I still get no output at all:(
Any ideas? The wii m+ is working!
What does your debug output look like?
Maybe you need external power for the servo. It's most likely not working because it is drawing more mA's than Arduino can provide.
I'm no longer using a servo.
I just want the values for each axis to appear in the serial monitor.
Anybody wants to do that for me (incl. servo-out)? Will pay you! 
Nobody? 
Would be REALLY helpful!
Would be really helpful TO POST YOUR DEBUG PRINT OUTPUT
In your code I found this line:
Serial.begin(115200);
In your serial window I see 19200 baud.
Make these equal to each other and tell us what happens.
thanks!!!
that's what I get:
You've got real problems: I see strings like "Yaw0", "Pitch0" and "WM+ Tuning People Bot Control"; in your sketch, but I don't see them in your debug prints.
Sorry during the screenshot the data got refreshed, so the strings moved away.
I just re-soldered my M+ adapter, when I now plug it it I get no values at all, when I unplug it I get only 0s.
any ideas?
when I now plug it it I get no values at all, when I unplug it I get only 0s.
any ideas?
Check your I2C protocols.