req_angle=45; velocity_x=cos(req_angle*val); velocity_y=sin(req_angle*val); velocity_1 = - velocity_x*(sin(theta*val)) + velocity_y*(cos(theta*val)) + radius*omega; velocity_2 = - velocity_x*(sin((theta + 120)*val)) + velocity_y*(cos((theta + 120)*val)) + radius*omega; velocity_3 = - velocity_x*(sin((theta + 240)*val)) + velocity_y*(cos((theta + 240)*val)) + radius*omega;
These are my equations for the three wheels of the omni robot. Required angle signifies the angle in which we move with respect to the x axis. Theta stores the orientation of the bot using an imu, which is zero on the x axis. They are correct.
I dont know what do I do with the term omega and on what should I apply my PID correction.
Here is a rough and general code Ive written for PID which ofcourse doesnt run at the moment.
int pid() { //kp, ki, kd store some constant values int error = target_position - current_position; int integral = integral + error; int derivative = error - last_error; int pwm= (kp * error) + (ki*integral) + (kd*derivative); if(pwm>255) pwm=255; if(pwm<-255) pwm=-255; if(pwm>0) direction=clockwise; if(pwm<0) direction=anticlockwwise; else brake; int last_error = error; }