Arduino Forum

Using Arduino => Programming Questions => Topic started by: BillD on Jun 12, 2011, 06:30 am

Title: Need help with code for motor control
Post by: BillD on Jun 12, 2011, 06:30 am
So I am very new to writing code and need some help.  I am trying to use a wii nunchuck to control motors. using a dfrobot 2a motor controller.  I have code working that reads the nunchuck but am not sure how to alter to make the motors go.

here is the code I am using to read nunchuck
I need to know where and how to insert code to make arduino read wii info and translate it to motor info. 

Code: [Select]

// read out a Wii Nunchuck controller
// adapted to work with wireless Nunchuck controllers of third party vendors by Michael Dreher <michael@5dot1.de>

// adapt to your hardware config
#define POWER_VIA_PORT_C2_C3 1    // use port pins port C2 and C3 as power supply of the Nunchuck (direct plug using wiichuck adapter)
#define DEBUG_RCV_TEL 1

#define USE_NEW_WAY_INIT 1        // use "The New Way" of initialization <http://wiibrew.org/wiki/Wiimote#The_New_Way>
#define WII_IDENT_LEN ((byte)6)
#define WII_TELEGRAM_LEN ((byte)6)
#define WII_NUNCHUCK_TWI_ADR ((byte)0x52)

#include <Wire.h>
#include <string.h>
#include <utility\twi.h>
#undef int
#include <stdio.h>

uint8_t outbuf[WII_TELEGRAM_LEN]; // array to store arduino output
int cnt = 0;
int ledPin = 13;

void setup ()
{
  Serial.begin (115200);

#ifdef POWER_VIA_PORT_C2_C3    // power supply of the Nunchuck via port C2 and C3
  PORTC &=~ _BV(PORTC2);
  PORTC |=  _BV(PORTC3);
  DDRC |= _BV(PORTC2) | _BV(PORTC3);  // make outputs
  delay(100);  // wait for things to stabilize
#endif

  Wire.begin(); // initialize i2c
  // we need to switch the TWI speed, because the nunchuck uses Fast-TWI
  // normally set in hardware\libraries\Wire\utility\twi.c twi_init()
  // this is the way of doing it without modifying the original files
#define TWI_FREQ_NUNCHUCK 400000L
  TWBR = ((CPU_FREQ / TWI_FREQ_NUNCHUCK) - 16) / 2;

  nunchuck_init(0); // send the initialization handshake

  // display the identification bytes, must be "00 00 A4 20 00 00" for the Nunchuck
  byte i;
  if(readControllerIdent(outbuf) == 0)
  {
    Serial.print("Ident=");
    for (i = 0; i < WII_TELEGRAM_LEN; i++)
    {
      Serial.print(outbuf[i], HEX);
      Serial.print(' ');
    }
    Serial.println();
  }

  Serial.println("Finished setup");
}

// params:
//   timeout: abort when timeout (in ms) expires, 0 for unlimited timeout
//   return:  0 == ok, 1 == timeout
byte nunchuck_init (unsigned short timeout)
{
  byte rc = 1;

#ifndef USE_NEW_WAY_INIT
  // look at <http://wiibrew.org/wiki/Wiimote#The_Old_Way> at "The Old Way"
  Wire.beginTransmission (WII_NUNCHUCK_TWI_ADR); // transmit to device 0x52
  Wire.send (0x40); // sends memory address
  Wire.send (0x00); // sends sent a zero. 
  Wire.endTransmission (); // stop transmitting
#else
  // disable encryption
  // look at <http://wiibrew.org/wiki/Wiimote#The_New_Way> at "The New Way"

  unsigned long time = millis();
  do
  {
    Wire.beginTransmission (WII_NUNCHUCK_TWI_ADR); // transmit to device 0x52
    Wire.send (0xF0); // sends memory address
    Wire.send (0x55); // sends data. 
    if(Wire.endTransmission() == 0) // stop transmitting
    {
      Wire.beginTransmission (WII_NUNCHUCK_TWI_ADR); // transmit to device 0x52
      Wire.send (0xFB); // sends memory address
      Wire.send (0x00); // sends sent a zero. 
      if(Wire.endTransmission () == 0) // stop transmitting
      {
        rc = 0;
      }
    }
  }
  while (rc != 0 && (!timeout || ((millis() - time) < timeout)));
#endif

  return rc;
}


// params:
//   ident [out]: pointer to buffer where 6 bytes of identification is stored. Buffer must be at least 6 bytes long.
//                A list of possible identifications can be found here: <http://wiibrew.org/wiki/Wiimote#The_New_Way>
//   return:  0 == ok, 1 == error
byte readControllerIdent(byte* pIdent)
{
  byte rc = 1;

  // read identification
  Wire.beginTransmission (WII_NUNCHUCK_TWI_ADR); // transmit to device 0x52
  Wire.send (0xFA); // sends memory address of ident in controller
  if(Wire.endTransmission () == 0) // stop transmitting
  {
    byte i;
    Wire.requestFrom (WII_NUNCHUCK_TWI_ADR, WII_TELEGRAM_LEN); // request data from nunchuck
    for (i = 0; (i < WII_TELEGRAM_LEN) && Wire.available (); i++)
    {
      pIdent[i] = Wire.receive(); // receive byte as an integer
    }
    if(i == WII_TELEGRAM_LEN)
    {
      rc = 0;
    }
  }
  return rc;
}

void clearTwiInputBuffer(void)
{
  // clear the receive buffer from any partial data
  while( Wire.available ())
    Wire.receive ();
}


void send_zero ()
{
  // I don't know why, but it only works correct when doing this exactly 3 times
  // otherwise only each 3rd call reads data from the controller (cnt will be 0 the other times)
  for(byte i = 0; i < 3; i++)
  {
    Wire.beginTransmission (WII_NUNCHUCK_TWI_ADR); // transmit to device 0x52
    Wire.send (0x00); // sends one byte
    Wire.endTransmission (); // stop transmitting
    //delay(1);
  }
}

void loop ()
{
  delay (1000);
  send_zero (); // send the request for next bytes
  Wire.requestFrom (WII_NUNCHUCK_TWI_ADR, WII_TELEGRAM_LEN); // request data from nunchuck

  for (cnt = 0; (cnt < WII_TELEGRAM_LEN) && Wire.available (); cnt++)
  {
    outbuf[cnt] = nunchuk_decode_byte (Wire.receive ()); // receive byte as an integer
    digitalWrite (ledPin, HIGH); // sets the LED on
  }

  // debugging
#ifdef DEBUG_RCV_TEL
  Serial.print("avail=");
  Serial.print(Wire.available(), DEC);
  Serial.print("  cnt=");
  Serial.println(cnt, DEC);
#endif

  clearTwiInputBuffer();

  // If we recieved the 6 bytes, then go print them
  if (cnt >= WII_TELEGRAM_LEN)
  {
    print ();
  }

}

// Print the input data we have recieved
// accel data is 10 bits long
// so we read 8 bits, then we have to add
// on the last 2 bits.  That is why I
// multiply them by 2 * 2
void print ()
{
  int joy_x_axis = outbuf[0];
  int joy_y_axis = outbuf[1];
  int accel_x_axis = outbuf[2] * 2 * 2;
  int accel_y_axis = outbuf[3] * 2 * 2;
  int accel_z_axis = outbuf[4] * 2 * 2;

  int z_button = 0;
  int c_button = 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)
  {
    z_button = 1;
  }
  if ((outbuf[5] >> 1) & 1)
  {
    c_button = 1;
  }

  if ((outbuf[5] >> 2) & 1)
  {
    accel_x_axis += 2;
  }
  if ((outbuf[5] >> 3) & 1)
  {
    accel_x_axis += 1;
  }

  if ((outbuf[5] >> 4) & 1)
  {
    accel_y_axis += 2;
  }
  if ((outbuf[5] >> 5) & 1)
  {
    accel_y_axis += 1;
  }

  if ((outbuf[5] >> 6) & 1)
  {
    accel_z_axis += 2;
  }
  if ((outbuf[5] >> 7) & 1)
  {
    accel_z_axis += 1;
  }

  Serial.print (joy_x_axis, DEC);
  Serial.print ("\t");

  Serial.print (joy_y_axis, DEC);
  Serial.print ("\t");

  Serial.print (accel_x_axis, DEC);
  Serial.print ("\t");

  Serial.print (accel_y_axis, DEC);
  Serial.print ("\t");

  Serial.print (accel_z_axis, DEC);
  Serial.print ("\t");

  Serial.print (z_button, DEC);
  Serial.print ("\t");

  Serial.print (c_button, DEC);
  Serial.print ("\t");

  Serial.print ("\r\n");
}


// Decode data format that original Nunchuck uses with old init sequence. This never worked with
// other controllers (e.g. wireless Nunchuck from other vendors)
char nunchuk_decode_byte (char x)
{
#ifndef USE_NEW_WAY_INIT
  x = (x ^ 0x17) + 0x17;
#endif
  return x;
}


also here is info on code for dfrobot 2a motor controller sample code from user manual
Code: [Select]

//Arduino PWM Speed Control?
int E1 = 6;   
int M1 = 7;
int E2 = 5;                         
int M2 = 4;                           

void setup()
{
    pinMode(M1, OUTPUT);   
    pinMode(M2, OUTPUT);
}

void loop()
{
  int value;
  for(value = 0 ; value <= 255; value+=5)
  {
    digitalWrite(M1,HIGH);   
    digitalWrite(M2, HIGH);       
    analogWrite(E1, value);   //PWM Speed Control
    analogWrite(E2, value);   //PWM Speed Control
    delay(30);
  } 
}


any help would be great thanks
Title: Re: Need help with code for motor control
Post by: PaulS on Jun 12, 2011, 02:14 pm
An accelerometer gives the instantaneous rate of change of velocity at a specific moment in time, in the X direction, the Y direction, and the Z direction.

How, exactly, do you want to use those three values to control the motor speeds and directions?

YOU have to provide the translation of acceleration data to motor speed/direction, or at least a description of which data you want to use, and how.
Title: Re: Need help with code for motor control
Post by: BillD on Jun 12, 2011, 04:12 pm
Thanks for the help Paul I'll try and give you some more information.

I am just starting simple right now I wasn't even going to use the accerometer info just the joystick right now

so the joy_y_axis reads 130 with nothing happening 131 to 255 is forward and 129 to 0 is reverse. 
The Joy_x_axis would be for turning 129 to 0 is left 130 nothing 131 to 255 is right

basically the set up is two motors a left and right M1 and M2 so I'd like to translate these numbers to pwm control of the motors.  if Joy_y_axis reads 255 both motors go forward etc

Joy_x_axis reads above 130 it would turn right so the left motor would run or M2 and less then 130 would be left and only M1 would run

the 2nd code gives the motor info pins and the setup

so an example would be if joy_y_axis is reading 200 that should be forward at half speed.  I am just not exactly sure how to do that.  I think it should translate to E1 and E2 values on the sample motor code.
Title: Re: Need help with code for motor control
Post by: PaulS on Jun 12, 2011, 06:16 pm
The values in outbuf[0] and outbuf[1] are the joystick values. So, what you want to do is compare the values to 130. If they are less than 130, set the direction HIGH. Otherwise set the direction LOW (these are the M1 and M2 pin's values).

Then, if the value is less than 130, map it to 255 to 0, from 0 to 130. If the value is greater than 130, map it to 0 to 255, from 130 to 255.

Pass the mapped value to analogWrite() for the En pin's values.
Title: Re: Need help with code for motor control
Post by: BillD on Jun 13, 2011, 07:33 pm
Thanks Paul