Drivetrain Pseudo Code

Drivetrain:

1. Move forwards (different speeds)
2. Move backward
3. Spin left (multiple angles)
4. Spin right (multiple angles)
5. Stop moving (Hard stop) <-- Need to ask Noodles

Motors: Victor SP (4 motors) One motor controller for each CIM motor

Sensors: Encoder, Gyro

Turn 180 degrees on the spot

One side moves forward, the other side moves backward The encoder checks whether the robot turns 180 degrees and stops the robot

• Number of rotations
• Find out how many turns are needed to spin 180 degrees
• Check if current number of rotations = number of rotations required

Turn 90 degrees on the spot Left or Right The encoder checks whether the robot turns 90 degrees and stops the robot

• Number of rotations
• Find out how many turns are needed to spin 90 degrees
• Check if current number of rotations = number of rotations required

The encoder checks whether the robot turns 360 degrees and stops the robot

• Number of rotations
• Find out how many turns are needed to spin 360 degrees
• Check if current number of rotations = number of rotations required

Driving Straight Pseudocode

Int encoder1 = value received from the first encoder (inches)
Int encoder2 = value received from the second encoder (inches)
Int motor1 = speed of the first motor (-1 to 1)
Int motor2 = speed of the second motor (-1 to 1)
Int speedChange = 0.5% of 1 (-1 to 0 or 0 to 1) * multiplier
Int multiplier = 0

Void Drivetrain::SetSpeed ()
{
Set the speed of the motors (in voltage)
}

Void Drivetrain::CheckEncoders()
{
If encoder1 == encoder2
{
Do nothing
}

Else
{
Go to AdjustEncoder
}
}

void Drivetrain::CheckDirection()
{
If motor1 = >0
{
multiplier = 1
}
Else
{
Multiplier = -1


Void Drivetrain::AdjustEncoder()
{
If encoder2 > encoder1
{
Decrease the speed of motor2 by speedChange
}

Else if (encoder1 > encoder2)
{
decrease the speed of motor1 by speedChange
}
}

Turn any angle based on the number of rotations

The encoder checks the number of rotations and sends a signal to the motor controllers to tell the motors to turn the robot a certain angle

1. Number of rotations
2. Find out how many turns are needed to spin a certain angle
3. Convert number of rotations into degrees and compared it to the certain angle we want to achieve