Go Down

Topic: Wii Motion Plus Servo Controller (Read 2 times) previous topic - next topic

RoHa

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!

RoHa

setup looks like this:

and that's the code I'm using:
Code: [Select]
//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);
}



Groove

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?
Per Arduino ad Astra

GaryP

How about the external power supply, are you running it with the USB-power only?

Kari
The only law for me; Ohms Law: U=R*I       P=U*I
Note to self: "Damn! Why don't you just fix it!!!"

RoHa

the servo is working. I now tested it on an 12V battery, still not working :(

AWOL

#5
Aug 23, 2010, 07:13 pm Last Edit: Aug 23, 2010, 07:15 pm by AWOL Reason: 1
Quote
. 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
"Pete, it's a fool looks for logic in the chambers of the human heart." Ulysses Everett McGill.
Do not send technical questions via personal messaging - they will be ignored.

RoHa

Thanks guys, it's a 4.8V servo.
I still get no output at all:(
Any ideas? The wii m+ is working!

Groove

What does your debug output look like?
Per Arduino ad Astra

CrashingDutchman

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.

RoHa

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!  ;)

RoHa

Nobody? :'(
Would be REALLY helpful!

AWOL

Would be really helpful TO POST YOUR DEBUG PRINT OUTPUT
"Pete, it's a fool looks for logic in the chambers of the human heart." Ulysses Everett McGill.
Do not send technical questions via personal messaging - they will be ignored.


CrashingDutchman

#13
Sep 12, 2010, 07:34 pm Last Edit: Sep 12, 2010, 07:35 pm by jds Reason: 1
In your code I found this line:
Code: [Select]
Serial.begin(115200);

In your serial window I see 19200 baud.

Make these equal to each other and tell us what happens.

RoHa


Go Up