Pages: [1] 2   Go Down
Author Topic: Wii Motion Plus Servo Controller  (Read 2142 times)
0 Members and 1 Guest are viewing this topic.
0
Offline Offline
Newbie
*
Karma: 0
Posts: 18
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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!
Logged

0
Offline Offline
Newbie
*
Karma: 0
Posts: 18
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

setup looks like this:

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

Logged

UK
Offline Offline
Faraday Member
**
Karma: 17
Posts: 2884
Gorm deficient
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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?
Logged

Per Arduino ad Astra

Espoo, Finland
Offline Offline
God Member
*****
Karma: 7
Posts: 586
"Oops, try again..."
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Kari
Logged


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

0
Offline Offline
Newbie
*
Karma: 0
Posts: 18
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

the servo is working. I now tested it on an 12V battery, still not working smiley-sad
Logged

Global Moderator
UK
Offline Offline
Brattain Member
*****
Karma: 302
Posts: 26281
I don't think you connected the grounds, Dave.
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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
« Last Edit: August 23, 2010, 12:15:21 pm by AWOL » Logged

"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.

0
Offline Offline
Newbie
*
Karma: 0
Posts: 18
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

UK
Offline Offline
Faraday Member
**
Karma: 17
Posts: 2884
Gorm deficient
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

What does your debug output look like?
Logged

Per Arduino ad Astra

Netherlands
Offline Offline
Sr. Member
****
Karma: 0
Posts: 414
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged

0
Offline Offline
Newbie
*
Karma: 0
Posts: 18
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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!  smiley-wink
Logged

0
Offline Offline
Newbie
*
Karma: 0
Posts: 18
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Nobody? :'(
Would be REALLY helpful!
Logged

Global Moderator
UK
Offline Offline
Brattain Member
*****
Karma: 302
Posts: 26281
I don't think you connected the grounds, Dave.
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Would be really helpful TO POST YOUR DEBUG PRINT OUTPUT
Logged

"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.

0
Offline Offline
Newbie
*
Karma: 0
Posts: 18
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Logged

Netherlands
Offline Offline
Sr. Member
****
Karma: 0
Posts: 414
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

In your code I found this line:
Code:
Serial.begin(115200);

In your serial window I see 19200 baud.

Make these equal to each other and tell us what happens.
« Last Edit: September 12, 2010, 12:35:18 pm by jds » Logged

0
Offline Offline
Newbie
*
Karma: 0
Posts: 18
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

thanks!!!
that's what I get:
Logged

Pages: [1] 2   Go Up
Jump to: