Kiwidrive code..

Here is the function for anyone interested. It controls the motor powers for the motors so that you can just point in a direction with your joystick and it will go that direction without rotating. The program uses trig to make the motors spin at the correct speed. Enjoy!

#include "Main.h"
void MotorPowers ( void )
{
// Assign values to trig globals (doubles)
cos30 = Cos ( 30 , 0 ) ;
// Calculate power for motor 1
Motor1 = (-1 * cos30 * JOYX) + (-1 * cos30 * JOYY) - ROT ; // calculates for both the x and y values
// Calculate power for motor 2
Motor2 = (-1 * cos30 * JOYX) + (cos30 * JOYY) - ROT ;
// Calculate power for motor 3
Motor3 = -(JOYX - ROT) ;
// Motor 1 Limits
if ( Motor1 >= 127 )
{
Motor1 = 127 ;
}
if ( Motor1 <= -127 )
{
Motor1 = -127 ;
}
// Motor 2 Limits
if ( Motor2 >= 127 )
{
Motor2 = 127 ;
}
if ( Motor2 <= -127 )
{
Motor2 = -127 ;
}
// Motor 3 Limits
if ( Motor3 >= 127 )
{
Motor3 = 127 ;
}
if ( Motor3 <= -127 )
{
Motor3 = -127 ;
}
}

Leave a Comment





Please note: Comment moderation is enabled and may delay your comment. There is no need to resubmit your comment.