Go Down

Topic: Mecanum/Holonomic Drive (Read 741 times) previous topic - next topic

BurtonCustomX

My current project uses an Arduino Uno with Ethernet Shield and a RobotOpen shield (http://www.team221.com/robotopen/product.php?id=105) but they don't have Mecanum (sometimes called Holonomic) drive examples, only Tank, Swerve, and Arcade style drive systems. So this is the robots current Tank setup:
Code: [Select]

#include <SPI.h>
#include <Ethernet.h>
#include <RobotOpen.h>


/* I/O Setup */
USBJoystick usb1('0');  // Assign the logitech USBJoystick object to bundle 0


void setup()
{
  /* Initiate comms */
  RobotOpen.begin();
}


/* This is your primary robot loop - all of your code
* should live here that allows the robot to operate
*/
void enabled() {
  // Constantly update PWM values with joystick values
  RobotOpen.setPWM(SIDECAR_PWM1, usb1.makePWM(ANALOG_LEFTY, NORMAL));
  RobotOpen.setPWM(SIDECAR_PWM2, usb1.makePWM(ANALOG_RIGHTY, NORMAL));
}


/* This is called while the robot is disabled
* You must make sure to set all of your outputs
* to safe/disable values here
*/
void disabled() {
  // PWMs are automatically disabled
}


/* This loop ALWAYS runs - only place code here that can run during a disabled state
* This is also a good spot to put driver station publish code
* You can use either publishAnalog, publishDigital, publishByte, publishShort, or publishLong
* Specify a bundle ID with a single character (a-z, A-Z, 0-9) - Just make sure not to use the same twice!
*/
void timedtasks() {
  RobotOpen.publishAnalog(ANALOG0, 'A');   // Bundle A
  RobotOpen.publishAnalog(ANALOG1, 'B');   // Bundle B
  RobotOpen.publishAnalog(ANALOG2, 'C');   // Bundle C
  RobotOpen.publishAnalog(ANALOG3, 'D');   // Bundle D
  RobotOpen.publishAnalog(ANALOG4, 'E');   // Bundle E
  RobotOpen.publishAnalog(ANALOG5, 'F');   // Bundle F
}


/* This is the main program loop that keeps comms operational
* There's no need to touch anything here!!!
*/
void loop() {
  RobotOpen.pollDS();
  if (RobotOpen.enabled())
    enabled();
  else
    disabled();
  timedtasks();
  RobotOpen.outgoingDS();
}


And my question is wether i could take the drive part out of Kiwi library's basic mecanum code

Code: [Select]
/* Kiwi drive macros and functions
* By Jonathan M. Guberman, 2011
* http://upnotnorth.net
*
* Use this code however you like, but don't blame me if it doesn't work!
*/

#include <Servo.h>

#define SPEED(x) (90 + (x))
#define DRIVE(x,y,z)   servo1.write(90 + (x)); servo2.write(90 + (y));servo3.write(90 + (z))

#define MAXSPEED 20

Servo servo1;
Servo servo2;
Servo servo3;

void setup()
{

}


void loop()
{

}

void setDrive(int angle, int maxspeed){
  float x = cos(3.14159 * (float) angle / 180);
  float y = sin(3.14159 * (float) angle / 180);
 
  DRIVE((int)(x*maxspeed),(int)((-0.5*x + 0.866*y)*maxspeed),(int)((-0.5*x-0.866*y)*maxspeed));
}

void setDrive(int angle){
  setDrive(angle, MAXSPEED);
}

void stopDrive(){
  DRIVE(0,0,0);
}

void stopDrive(int time){
  stopDrive();
  delay(time);
}


And copy & paste it into the RobotOpen code, replacing the tank drive bit. This possible? and where would the code be inserted?

Go Up