Fall 2016 Velociraptor (W): Control Algorithm #1
By: Taylor Farr (E&C)
Approved by Ryan Daly (Division Manager for Electronics and Controls)
Lam Nguyen (Project Manager)
Table of Contents
Fulfilled Requirements
- The Velociraptor shall statically walk.
- The Velociraptor should dynamically walk
In order to fulfill these requirements, a software algorithm needs to be implemented that reads the rotary sensors and move the dc motors and servos such that it is able to walk. Since the Velociraptor is using dc motors for the legs, rotary encoders need to be utilized so that the system knows the rpm and position of the motors at all times. No two dc motors are the same, so the pwm signal sent to the motors will vary for each otherwise the robot will be unable to walk. For our final design, we intend to use potentiometers for our rotary sensors.
Critical Design Review – Control Algorithm
Approach
For our CDR demonstration, we intended to use the potentiometers, however, this proved to be difficult to implement due to the size. Instead, we used optical sensors. The optical sensors behave like a switch. An led emits light while a BJT senses light. We then incorporated two circles with black ink across the diameter.
Materials
- 2 gm9’s (DC motors)
- 2 optical sensors
- 1 servo
- 3DoT Board
Control Algorithm – Summary
The algorithm is simple:
- Move the servo 30 degrees left (This will shift the head and tail over the left leg.).
- Move both dc motors 180 degrees (This will shift the right leg forward as the left leg moves and holds all the weight).
- Move the servo 60 degrees right (so the center of mass is over the right leg).
- Move both dc motors 180 degrees (This will shift the left leg forward as the right leg moves and holds all the weight).
- End
The algorithm uses a for loop to gradually move the servo to the desired position. Following that is a while loop. In the while loop, both motors move forward until the optoreflective switch reads a 0 output (An output of 1 indicates that the switch is looking at a white surface, and an output of 0 indicates the black line). There are two switches that need to be read, so some if statements are necessary in case one motor has moved 180 degrees before the other motor. After each sensor reads 0, each motor will stop. This will then use the for loop again to move the servo in the opposite direction. This subroutine is under moveHandler so that the systems engineer can use their Arxterra app to call the function using Bluetooth.
Figure 1 – Dc Motor with Optical Encoders
Control Algorithm – Code
void moveHandler (uint8_t cmd, uint8_t param[], uint8_t n)
{
ReadIR();
for (pos = pos; pos<=120; pos++)
{
servo_pin_13.write(pos);
delay(15);
SoftwareServo::refresh();
}
ReadIR();
motorA.go(param[0],0x40);// go(direction,pwm) –> direction: FORWARD =1, BACKWARD =2, BRAKE = 3, RELEASE = 4; pwm range(0-255) used to control the speed of the motor
//in my case I used 1 to move forward and a 255 for max speed
motorB.go(param[2],0x40);// go(direction,pwm) –> direction: FORWARD =1, BACKWARD =2, BRAKE = 3, RELEASE = 4; pwm range(0-255) used to control the speed of the motor
delay(500); // need to calculate delay
ReadIR();
// IR = digitalRead(sig);
//Serial.println(IR);
// IR2 = digitalRead(sig2);
while ((IR!=0)&&(IR2!=0))
{
motorA.go(param[0],0x40);// go(direction,pwm) –> direction: FORWARD =1, BACKWARD =2, BRAKE = 3, RELEASE = 4; pwm range(0-255) used to control the speed of the motor
//in my case I used 1 to move forward and a 255 for max speed
// IR = digitalRead(sig);
ReadIR();
motorB.go(param[2],0x40);// go(direction,pwm) –> direction: FORWARD =1, BACKWARD =2, BRAKE = 3, RELEASE = 4; pwm range(0-255) used to control the speed of the motor
// IR2 = digitalRead(sig2);
ReadIR();
}
if ((IR==0)&&(IR2!=0))
{
motorA.brake();
ReadIR();
while (IR2!=0)
{
motorB.go(param[2],0x40);// go(direction,pwm) –> direction: FORWARD =1, BACKWARD =2, BRAKE = 3, RELEASE = 4; pwm range(0-255) used to control the speed of the motor
// IR2 = digitalRead(sig2);
ReadIR();
}
if (IR2==0)
{
motorB.brake();
ReadIR();
}
}
if ((IR!=0)&&(IR2==0))
{
motorB.brake();
ReadIR();
while (IR!=0)
{
motorA.go(param[0],0x40);// go(direction,pwm) –> direction: FORWARD =1, BACKWARD =2, BRAKE = 3, RELEASE = 4; pwm range(0-255) used to control the speed of the motor
// IR = digitalRead(sig);
ReadIR();
}
if (IR==0)
{
motorA.brake();
ReadIR();
}
}
//IR = digitalRead(sig);
//IR2 = digitalRead(sig2);
ReadIR();
for (pos=pos; pos>=60; pos–)
{
servo_pin_13.write(pos);
delay(15);
SoftwareServo::refresh();
}
motorA.go(param[0],0x40);// go(direction,pwm) –> direction: FORWARD =1, BACKWARD =2, BRAKE = 3, RELEASE = 4; pwm range(0-255) used to control the speed of the motor
//in my case I used 1 to move forward and a 255 for max speed
motorB.go(param[2],0x40);// go(direction,pwm) –> direction: FORWARD =1, BACKWARD =2, BRAKE = 3, RELEASE = 4; pwm range(0-255) used to control the speed of the motor
delay(500); // need to calculate delay
// IR = digitalRead(sig);
// Serial.println(IR);
// IR2 = digitalRead(sig2);
ReadIR();
while ((IR!=0)&&(IR2!=0))
{
motorA.go(param[0],0x40);// go(direction,pwm) –> direction: FORWARD =1, BACKWARD =2, BRAKE = 3, RELEASE = 4; pwm range(0-255) used to control the speed of the motor
//in my case I used 1 to move forward and a 255 for max speed
// IR = digitalRead(sig);
ReadIR();
motorB.go(param[2],0x40);// go(direction,pwm) –> direction: FORWARD =1, BACKWARD =2, BRAKE = 3, RELEASE = 4; pwm range(0-255) used to control the speed of the motor
// IR2 = digitalRead(sig2);
ReadIR();
}
ReadIR();
if ((IR==0)&&(IR2!=0))
{
motorA.brake();
ReadIR();
while (IR2!=0)
{
motorB.go(param[2],0x40);// go(direction,pwm) –> direction: FORWARD =1, BACKWARD =2, BRAKE = 3, RELEASE = 4; pwm range(0-255) used to control the speed of the motor
// IR2 = digitalRead(sig2);
ReadIR();
}
if (IR2==0)
{
motorB.brake();
ReadIR();
}
}
if ((IR!=0)&&(IR2==0))
{
motorB.brake();
ReadIR();
while (IR!=0)
{
motorA.go(param[0],0x40);// go(direction,pwm) –> direction: FORWARD =1, BACKWARD =2, BRAKE = 3, RELEASE = 4; pwm range(0-255) used to control the speed of the motor
// IR = digitalRead(sig);
ReadIR();
}
if (IR==0)
{
motorA.brake();
ReadIR();
}
}
} // moveHandler
void ReadIR()
{
// Read in the ADC and convert it to a voltage:
int proximityADC = analogRead(QRD1114_PIN);
int proximityADC2 = analogRead(QRD1114_PIN2);
if (proximityADC < 630){
IR = HIGH;
}
else {
IR = LOW;
}
if (proximityADC2 < 630){
IR2 = HIGH;
}
else {
IR2 = LOW;
}
Conclusion
We were able to get the motors to move successfully. However, problems arose when we connected this to our prototype. There was more friction in the wheels than anticipated. Another issue is making sure that the legs start off in the correct position. If not, it is very likely that the robot will become unstable, and fall over.