Forum >Replies by Swanglei
Replies （3）
- Topic: DIY Omni wheels robotYou Reply: /*********************************************************************/
Code Author:DFRobot-Kelvin
Control Protocol?x100-y100-w100
X Direction:100mm/s
Y Direction:100mm/s
rotation to the left:100mm/s
/*********************************************************************/
#include <Metro.h>
double AcceleratedSpeed=400,whieelAcceleratedSpeed=500;//
double Step=0.03;//15*3.14/200/16
int xPul=25,
xDir=23,
xEnable=27,
yPul=31,
yDir=33,
yEnable=29,
zPul=37,
zDir=39,
zEnable=35,
km_1=8;
Metro _Serial_=Metro(20);
double xspeed,yspeed,zspeed;
static double xSpeed,ySpeed,wheel;
long x_t,y_t,z_t;
void setup() {
// put your setup code here, to run once:
Serial3.begin(115200);
pinMode(8,OUTPUT);
pinMode(xPul,OUTPUT);
pinMode(xDir,OUTPUT);
pinMode(yPul,OUTPUT);
pinMode(yDir,OUTPUT);
pinMode(zPul,OUTPUT);
pinMode(zDir,OUTPUT);
pinMode(xEnable,OUTPUT);
pinMode(yEnable,OUTPUT);
pinMode(zEnable,OUTPUT);
digitalWrite(xEnable,LOW);
digitalWrite(yEnable,LOW);
digitalWrite(zEnable,LOW);
digitalWrite(km_1,HIGH);
}
void loop() {
// put your main code here, to run repeatedly:
static long t;
if(_Serial_.check()==1)
{
serial();
Speed(xSpeed,ySpeed,AcceleratedSpeed);
// Wheel(wheel);
if(xspeed!=0)
x_t=int(Step/abs(xspeed)*1000000);
if(yspeed!=0)
y_t=int(Step/abs(yspeed)*1000000);
if(zspeed!=0)
z_t=int(Step/abs(zspeed)*1000000);
if(xspeed>0)
digitalWrite(xDir,HIGH);
else
digitalWrite(xDir,LOW);
if(yspeed>0)
digitalWrite(yDir,HIGH);
else
digitalWrite(yDir,LOW);
if(zspeed>0)
digitalWrite(zDir,LOW);
else
digitalWrite(zDir,HIGH);
}
xMove(xspeed);
yMove(yspeed);
zMove(zspeed);
}
void serial()
{
if(Serial3.available()==0) return;
char buf=Serial3.read();
switch(buf)
{
case 120://x
xSpeed=Serial3.parseInt();
break;
case 121://y
ySpeed=Serial3.parseInt();
break;
case 119://w
wheel=Serial3.parsedouble();
break;
}
}
void Speed(double XSpeed,double YSpeed,double a)
{
static double S_XSpeed=0,S_YSpeed=0;
if (XSpeed>S_XSpeed)
{
S_XSpeed=S_XSpeed+a*0.02;
if(S_XSpeed>XSpeed)
{
S_XSpeed=XSpeed;
}
}
else{
S_XSpeed=S_XSpeed-a*0.02;
if(S_XSpeed<XSpeed)
{
S_XSpeed=XSpeed;
}
}
if (YSpeed>S_YSpeed)
{
S_YSpeed=S_YSpeed+a*0.02;
if(S_YSpeed>YSpeed)
{
S_YSpeed=YSpeed;
}
}
else{
S_YSpeed=S_YSpeed-a*0.02;
if(S_YSpeed<YSpeed)
{
S_YSpeed=YSpeed;
}
}
xspeed=0.5*(S_XSpeed**.732*S_YSpeed);
yspeed=0.5*(S_XSpeed-1.732*S_YSpeed);
zspeed=S_XSpeed;
}
void Wheel(double _wheel)
{
static double wheelspeed=0;
if(_wheel>wheelspeed)
{
wheelspeed=wheelspeed+whieelAcceleratedSpeed*0.02;
if(_wheel<wheelspeed)
{
wheelspeed=_wheel;
}
}
else
{
wheelspeed=wheelspeed-whieelAcceleratedSpeed*0.02;
if(_wheel>wheelspeed)
{
wheelspeed=_wheel;
}
}
xspeed=xspeed+wheelspeed;
yspeed=yspeed+wheelspeed;
zspeed=zspeed-wheelspeed;
}
void xMove(double xs)
{
static long x_time=micros();
if(xs!=0)
{
if(micros()-x_time>=x_t)
{
digitalWrite(xPul,HIGH);
digitalWrite(xPul,LOW);
x_time=micros();
}
}
}
void yMove(double ys)
{
static long y_time=micros();
if(ys!=0)
{
if(micros()-y_time>=y_t)
{
digitalWrite(yPul,HIGH);
digitalWrite(yPul,LOW);
y_time=micros();
}
}
}
void zMove(double zs)
{
static long z_time=micros();
if(zs!=0)
{
if(micros()-z_time>=z_t)
{
digitalWrite(zPul,HIGH);
digitalWrite(zPul,LOW);
z_time=micros();
}
}
} - Topic: DIY Omni wheels robotYou Reply: And the PCB design
I used heat transfer machine to make the pcb and drill the holes(make me crazy) - Topic: DIY Omni wheels robotYou Reply: And here we go......
the ommi wheels.....