Unit 3: Robotic Movement 2 – Affectors

This entry deals with affectors. Affectors allow a robot to affect or interact with its environment. As usual I will begin with the readings, and then spend some time working through the final circuits in the SIK. Once these have been completed I will work through some questions from the instructors notebook.

Reading – Chapter 6: Grasping at Straws

This chapter examines Manipulators and Affectors and dives into the difference between teleoperation and remote control. (Spoiler alert – Remote control is basically the same as teleoperation but only used to describe toys and simple systems. Teleoperation is used to make the same concept sound more mature!) The concepts are explored a bit and the implications are examined including the need for and challenge of inverse kinematics for positioning the affectors.

  • How many DOF are there in a human hand? can you control each of them independently?

I believe we briefly discussed this question in the previous unit, where we established that there are 27 degrees of freedom in the human hand. I would argue that yes it is possible to control each of the DOF individually. Typically I believe that we just “bend” or flex the finger, but it is possible to bend at each joint. As such I do believe that it is possible to control each DoF in the hand, though it does take more concentration to bend individual joints rather than to just flex the finger as a whole.

Each finger is composed of three joints as illustrated in this figure:

https://en.wikipedia.org/wiki/File:DIP,_PIP_and_MCP_joints_of_hand.jpg

The DIP is the Distal Interphalangeal joint, the PIP is the proximal interphalangeal joint, and the MCP is the Metacarpophalangeal joint. Here are pictures of each joint individually flexed, and an overall flex or bend to a finger.

Fully bent finger
MCP bend
PIP Bend
DIP bend, notice the reduced range of motion, but it is still possible to flex this joint.
  • Which of the two joint types we discussed, rotational and prismatic, is more commonly found in biological bodies? Can you think of examples of specific animals?

Rotary joints provide rotational movement around a fixed axis and are the primary way that biological systems use joints. Biological systems make extensive use of lever arms and basic physics to increase the force exerted across a rotational joint where two bones meet and are cushioned and lubricated by cartilage to allow rotation in relation to each other. I cannot think of an example of a prismatic joint in an animal, but examples of rotational joints abound knees, elbows, jaws and more.

  • Are astronaut suits exoskeletons? What about a lever-controlled backhoe?

An exoskeleton is a shell that provides protection or support and is not able to make its own decisions or act on them without an operator/occupant. So astronaut suits are exoskeletons, while a lever-controlled backhoe is not an exoskeleton, as it is not a shell around a user that provides protection or support. It would be more accurately described as a teleoperated machine.

  • Imagine an exoskeleton which has its own sensors, makes its own decisions, and acts on them. This is definitely a robot, yet it is also controlled by a human and attached to the human’s body. Can you imagine where this could be useful?

Science fiction is full of suits such as this, exoskeletons that can provide the wearer with enhanced speed, strength, ability or skill. By definition I believe the power loader suit from the aliens franchise would qualify, or the Master Chiefs armour from the halo universe, or Gordon Freemans HEV Suit. These suits give their wearers protection and support from their environment, while at the same time they are equipped with sensors and processors that can automate or achieve tasks necessary to save their occupant. The HEV suit can administer first aid if trauma is experienced, the Spartan armour of the master chief can interface with artificial intelligence constructs that can use the machinery inside his exoskeleton to enhance and augment his abilities and capabilities.

  • Robotic dogs have been used to play soccer. Because they are not designed for that task, they move slowly and have trouble aiming and kicking the ball (not to mention finding it, but that’s not part of manipulation; it belongs to sensing). Some of the best robot soccer teams have come up with a clever idea: instead of having the dogs walk as they normally do, they lower them so that the two rear legs are driving and steering the robot, while the two front legs are on the ground used to grasp the ball and kick it, which is much easier. This turns out to work very well, even if it looks rather silly. What are the end effectors of the dogs in that case? Are the dogs now purely mobile robots?

I had to look up videos on YouTube of robot dogs playing soccer, and I found some older videos that exhibit this behavior. There were also some newer videos from MIT where robotic dogs were playing soccer in the way you would expect them to. In the first case, the robots were using their rear legs for mobility, and their front legs as a manipulator that can grab and then kick the ball.

Project 5: Robot

The next three circuits add some motors to the sparkfun and breadboard which can then be used to unleash my robotic creation upon the world, Muahahaha…

Circuit 5A: Motor Basics

This first project is an introduction to the motors and motor drivers and how the project works together.

The wiring diagram for circuit 5a.
The wired up circuit for 5A
/*
  SparkFun Inventor’s Kit
  Circuit 5A - Motor Basics

  Learn how to control one motor with the motor driver.

  This sketch was written by SparkFun Electronics, with lots of help from the Arduino community.
  This code is completely free for any use.

  View circuit diagram and instructions at: https://learn.sparkfun.com/tutorials/sparkfun-inventors-kit-experiment-guide---v41
  Download drawings and code at: https://github.com/sparkfun/SIK-Guide-Code
*/

//PIN VARIABLES
//the motor will be controlled by the motor A pins on the motor driver
const int AIN1 = 13;           //control pin 1 on the motor driver for the right motor
const int AIN2 = 12;            //control pin 2 on the motor driver for the right motor
const int PWMA = 11;            //speed control pin on the motor driver for the right motor

int switchPin = 7;             //switch to turn the robot on and off

//VARIABLES
int motorSpeed = 0;       //starting speed for the motor

void setup() {
  pinMode(switchPin, INPUT_PULLUP);   //set this as a pullup to sense whether the switch is flipped

  //set the motor control pins as outputs
  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);
  pinMode(PWMA, OUTPUT);

  Serial.begin(9600);                       //begin serial communication with the computer

  Serial.println("Enter motor speed (0-255)... ");    //Prompt to get input in the serial monitor.
}

void loop() {

  if (Serial.available() > 0) {         //if the user has entered something in the serial monitor
    motorSpeed = Serial.parseInt();     //set the motor speed equal to the number in the serial message

    Serial.print("Motor Speed: ");      //print the speed that the motor is set to run at
    Serial.println(motorSpeed);
  }

  if (digitalRead(7) == LOW) {          //if the switch is on...
    spinMotor(motorSpeed);
  } else {                              //if the switch is off...
    spinMotor(0);                   //turn the motor off
  }


}

/********************************************************************************/
void spinMotor(int motorSpeed)                       //function for driving the right motor
{
  if (motorSpeed > 0)                                 //if the motor should drive forward (positive speed)
  {
    digitalWrite(AIN1, HIGH);                         //set pin 1 to high
    digitalWrite(AIN2, LOW);                          //set pin 2 to low
  }
  else if (motorSpeed < 0)                            //if the motor should drive backward (negative speed)
  {
    digitalWrite(AIN1, LOW);                          //set pin 1 to low
    digitalWrite(AIN2, HIGH);                         //set pin 2 to high
  }
  else                                                //if the motor should stop
  {
    digitalWrite(AIN1, LOW);                          //set pin 1 to low
    digitalWrite(AIN2, LOW);                          //set pin 2 to low
  }
  analogWrite(PWMA, abs(motorSpeed));                 //now that the motor direction is set, drive it at the entered speed
}


The circuit in action

Circuit 5B: Remote Controlled Robot

This circuit mounts two motors onto the plate and allows for controls to be sent to the robot to control it.

Wiring Diagram for circuit 5B
Circuit 5B wired up
/*
  SparkFun Inventor’s Kit
  Circuit 5B - Remote Control Robot

  Control a two wheeled robot by sending direction commands through the serial monitor.
  This sketch was adapted from one of the activities in the SparkFun Guide to Arduino.
  Check out the rest of the book at
  https://www.sparkfun.com/products/14326

  This sketch was written by SparkFun Electronics, with lots of help from the Arduino community.
  This code is completely free for any use.

  View circuit diagram and instructions at: https://learn.sparkfun.com/tutorials/sparkfun-inventors-kit-experiment-guide---v41
  Download drawings and code at: https://github.com/sparkfun/SIK-Guide-Code
*/


//the right motor will be controlled by the motor A pins on the motor driver
const int AIN1 = 13;           //control pin 1 on the motor driver for the right motor
const int AIN2 = 12;            //control pin 2 on the motor driver for the right motor
const int PWMA = 11;            //speed control pin on the motor driver for the right motor

//the left motor will be controlled by the motor B pins on the motor driver
const int PWMB = 10;           //speed control pin on the motor driver for the left motor
const int BIN2 = 9;           //control pin 2 on the motor driver for the left motor
const int BIN1 = 8;           //control pin 1 on the motor driver for the left motor

int switchPin = 7;             //switch to turn the robot on and off

const int driveTime = 20;      //this is the number of milliseconds that it takes the robot to drive 1 inch
                               //it is set so that if you tell the robot to drive forward 25 units, the robot drives about 25 inches

const int turnTime = 8;        //this is the number of milliseconds that it takes to turn the robot 1 degree
                               //it is set so that if you tell the robot to turn right 90 units, the robot turns about 90 degrees

                               //Note: these numbers will vary a little bit based on how you mount your motors, the friction of the
                               //surface that your driving on, and fluctuations in the power to the motors.
                               //You can change the driveTime and turnTime to make them more accurate

String botDirection;           //the direction that the robot will drive in (this change which direction the two motors spin in)
String distance;               //the distance to travel in each direction

/********************************************************************************/
void setup()
{
  pinMode(switchPin, INPUT_PULLUP);   //set this as a pullup to sense whether the switch is flipped

  //set the motor control pins as outputs
  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);
  pinMode(PWMA, OUTPUT);

  pinMode(BIN1, OUTPUT);
  pinMode(BIN2, OUTPUT);
  pinMode(PWMB, OUTPUT);

  Serial.begin(9600);           //begin serial communication with the computer

  //prompt the user to enter a command
  Serial.println("Enter a direction followed by a distance.");
  Serial.println("f = forward, b = backward, r = turn right, l = turn left");
  Serial.println("Example command: f 50");
}

/********************************************************************************/
void loop()
{
  if (digitalRead(7) == LOW)
  {                                                     //if the switch is in the ON position
    if (Serial.available() > 0)                         //if the user has sent a command to the RedBoard
    {
      botDirection = Serial.readStringUntil(' ');       //read the characters in the command until you reach the first space
      distance = Serial.readStringUntil(' ');           //read the characters in the command until you reach the second space

      //print the command that was just received in the serial monitor
      Serial.print(botDirection);
      Serial.print(" ");
      Serial.println(distance.toInt());

      if (botDirection == "f")                         //if the entered direction is forward
      {
        rightMotor(200);                                //drive the right wheel forward
        leftMotor(200);                                 //drive the left wheel forward
        delay(driveTime * distance.toInt());            //drive the motors long enough travel the entered distance
        rightMotor(0);                                  //turn the right motor off
        leftMotor(0);                                   //turn the left motor off
      }
      else if (botDirection == "b")                    //if the entered direction is backward
      {
        rightMotor(-200);                               //drive the right wheel forward
        leftMotor(-200);                                //drive the left wheel forward
        delay(driveTime * distance.toInt());            //drive the motors long enough travel the entered distance
        rightMotor(0);                                  //turn the right motor off
        leftMotor(0);                                   //turn the left motor off
      }
      else if (botDirection == "r")                     //if the entered direction is right
      {
        rightMotor(-200);                               //drive the right wheel forward
        leftMotor(255);                                 //drive the left wheel forward
        delay(turnTime * distance.toInt());             //drive the motors long enough turn the entered distance
        rightMotor(0);                                  //turn the right motor off
        leftMotor(0);                                   //turn the left motor off
      }
      else if (botDirection == "l")                   //if the entered direction is left
      {
        rightMotor(255);                                //drive the right wheel forward
        leftMotor(-200);                                //drive the left wheel forward
        delay(turnTime * distance.toInt());             //drive the motors long enough turn the entered distance
        rightMotor(0);                                  //turn the right motor off
        leftMotor(0);                                   //turn the left motor off
      }
    }
  }
  else
  {
    rightMotor(0);                                  //turn the right motor off
    leftMotor(0);                                   //turn the left motor off
  }
}
/********************************************************************************/
void rightMotor(int motorSpeed)                       //function for driving the right motor
{
  if (motorSpeed > 0)                                 //if the motor should drive forward (positive speed)
  {
    digitalWrite(AIN1, HIGH);                         //set pin 1 to high
    digitalWrite(AIN2, LOW);                          //set pin 2 to low
  }
  else if (motorSpeed < 0)                            //if the motor should drive backward (negative speed)
  {
    digitalWrite(AIN1, LOW);                          //set pin 1 to low
    digitalWrite(AIN2, HIGH);                         //set pin 2 to high
  }
  else                                                //if the motor should stop
  {
    digitalWrite(AIN1, LOW);                          //set pin 1 to low
    digitalWrite(AIN2, LOW);                          //set pin 2 to low
  }
  analogWrite(PWMA, abs(motorSpeed));                 //now that the motor direction is set, drive it at the entered speed
}

/********************************************************************************/
void leftMotor(int motorSpeed)                        //function for driving the left motor
{
  if (motorSpeed > 0)                                 //if the motor should drive forward (positive speed)
  {
    digitalWrite(BIN1, HIGH);                         //set pin 1 to high
    digitalWrite(BIN2, LOW);                          //set pin 2 to low
  }
  else if (motorSpeed < 0)                            //if the motor should drive backward (negative speed)
  {
    digitalWrite(BIN1, LOW);                          //set pin 1 to low
    digitalWrite(BIN2, HIGH);                         //set pin 2 to high
  }
  else                                                //if the motor should stop
  {
    digitalWrite(BIN1, LOW);                          //set pin 1 to low
    digitalWrite(BIN2, LOW);                          //set pin 2 to low
  }
  analogWrite(PWMB, abs(motorSpeed));                 //now that the motor direction is set, drive it at the entered speed
}
Circuit 5B in action

Circuit 5C: Autonomous Robot

And now we bring it all together with a robot that can move and avoid obstacles all on its own.

The wiring diagram for Circuit 5C.
The wired up circuit 5C.
/*
  SparkFun Inventor’s Kit
  Circuit 5C - Autonomous Robot

  This robot will drive around on its own and react to obstacles by backing up and turning to a new direction.
  This sketch was adapted from one of the activities in the SparkFun Guide to Arduino.
  Check out the rest of the book at
  https://www.sparkfun.com/products/14326

  This sketch was written by SparkFun Electronics, with lots of help from the Arduino community.
  This code is completely free for any use.

  View circuit diagram and instructions at: https://learn.sparkfun.com/tutorials/sparkfun-inventors-kit-experiment-guide---v41
  Download drawings and code at: https://github.com/sparkfun/SIK-Guide-Code
*/



//the right motor will be controlled by the motor A pins on the motor driver
const int AIN1 = 13;           //control pin 1 on the motor driver for the right motor
const int AIN2 = 12;            //control pin 2 on the motor driver for the right motor
const int PWMA = 11;            //speed control pin on the motor driver for the right motor

//the left motor will be controlled by the motor B pins on the motor driver
const int PWMB = 10;           //speed control pin on the motor driver for the left motor
const int BIN2 = 9;           //control pin 2 on the motor driver for the left motor
const int BIN1 = 8;           //control pin 1 on the motor driver for the left motor


//distance variables
const int trigPin = 6;
const int echoPin = 5;

int switchPin = 7;             //switch to turn the robot on and off

float distance = 0;            //variable to store the distance measured by the distance sensor

//robot behaviour variables
int backupTime = 300;           //amount of time that the robot will back up when it senses an object
int turnTime = 200;             //amount that the robot will turn once it has backed up

/********************************************************************************/
void setup()
{
  pinMode(trigPin, OUTPUT);       //this pin will send ultrasonic pulses out from the distance sensor
  pinMode(echoPin, INPUT);        //this pin will sense when the pulses reflect back to the distance sensor

  pinMode(switchPin, INPUT_PULLUP);   //set this as a pullup to sense whether the switch is flipped


  //set the motor control pins as outputs
  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);
  pinMode(PWMA, OUTPUT);

  pinMode(BIN1, OUTPUT);
  pinMode(BIN2, OUTPUT);
  pinMode(PWMB, OUTPUT);

  Serial.begin(9600);                       //begin serial communication with the computer
  Serial.print("To infinity and beyond!");  //test the serial connection
}

/********************************************************************************/
void loop()
{
  //DETECT THE DISTANCE READ BY THE DISTANCE SENSOR
  distance = getDistance();

  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" in");              // print the units

  if (digitalRead(switchPin) == LOW) { //if the on switch is flipped

    if (distance < 10) {              //if an object is detected
      //back up and turn
      Serial.print(" ");
      Serial.print("BACK!");

      //stop for a moment
      rightMotor(0);
      leftMotor(0);
      delay(200);

      //back up
      rightMotor(-255);
      leftMotor(-255);
      delay(backupTime);

      //turn away from obstacle
      rightMotor(255);
      leftMotor(-255);
      delay(turnTime);

    } else {                        //if no obstacle is detected drive forward
      Serial.print(" ");
      Serial.print("Moving...");


      rightMotor(255);
      leftMotor(255);
    }
  } else {                        //if the switch is off then stop

    //stop the motors
    rightMotor(0);
    leftMotor(0);
  }

  delay(50);                      //wait 50 milliseconds between readings
}

/********************************************************************************/
void rightMotor(int motorSpeed)                       //function for driving the right motor
{
  if (motorSpeed > 0)                                 //if the motor should drive forward (positive speed)
  {
    digitalWrite(AIN1, HIGH);                         //set pin 1 to high
    digitalWrite(AIN2, LOW);                          //set pin 2 to low
  }
  else if (motorSpeed < 0)                            //if the motor should drive backward (negative speed)
  {
    digitalWrite(AIN1, LOW);                          //set pin 1 to low
    digitalWrite(AIN2, HIGH);                         //set pin 2 to high
  }
  else                                                //if the motor should stop
  {
    digitalWrite(AIN1, LOW);                          //set pin 1 to low
    digitalWrite(AIN2, LOW);                          //set pin 2 to low
  }
  analogWrite(PWMA, abs(motorSpeed));                 //now that the motor direction is set, drive it at the entered speed
}

/********************************************************************************/
void leftMotor(int motorSpeed)                        //function for driving the left motor
{
  if (motorSpeed > 0)                                 //if the motor should drive forward (positive speed)
  {
    digitalWrite(BIN1, HIGH);                         //set pin 1 to high
    digitalWrite(BIN2, LOW);                          //set pin 2 to low
  }
  else if (motorSpeed < 0)                            //if the motor should drive backward (negative speed)
  {
    digitalWrite(BIN1, LOW);                          //set pin 1 to low
    digitalWrite(BIN2, HIGH);                         //set pin 2 to high
  }
  else                                                //if the motor should stop
  {
    digitalWrite(BIN1, LOW);                          //set pin 1 to low
    digitalWrite(BIN2, LOW);                          //set pin 2 to low
  }
  analogWrite(PWMB, abs(motorSpeed));                 //now that the motor direction is set, drive it at the entered speed
}

/********************************************************************************/
//RETURNS THE DISTANCE MEASURED BY THE HC-SR04 DISTANCE SENSOR
float getDistance()
{
  float echoTime;                   //variable to store the time it takes for a ping to bounce off an object
  float calculatedDistance;         //variable to store the distance calculated from the echo time

  //send out an ultrasonic pulse that's 10ms long
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  echoTime = pulseIn(echoPin, HIGH);      //use the pulsein command to see how long it takes for the
                                          //pulse to bounce back to the sensor

  calculatedDistance = echoTime / 148.0;  //calculate the distance of the object that reflected the pulse (half the bounce time multiplied by the speed of sound)

  return calculatedDistance;              //send back the distance that was calculated
}
The robot in action. On my desk hooked up to usb it was happy to go forward, but plugged in to the battery pack and on the floor, it keeps backing up for collision avoidance. I think there might be an issue with the placement of the ultrasonic sensor, or the carpet might be interfering.

Design Question

  • How many motors and of what type would you require to make a fully functional robotic arm that had a working elbow, wrist, and end effector(i.e. a simple clamp)? What components would you add if you wanted the clamp to be able to tell how hard it was grabbing an object such as an egg(i.e. to avoid crushing it)? Discuss your design in your weblog in detail, especially describing the choice of motor for each joint, the degrees of freedom and the range of motion.

A fully functional arm would take a lot of motors if it was intended to replicate the capability and range of motion of a full human arm and hand. An elbow joint can open and close it doesn’t rotate or move side to side, so it has one DoF. A wrist can bend forwards or backwards, flex left or right and rotate clockwise or counterclockwise so that is 3 DoF for a fully functional wrist. A basic clamp or pincer would be able to open or close so that is one DoF. If you were looking to recreate human style movement and anatomy, you would need to have either artificial muscles or hydraulics in opposing pairs to capture a similiar movement system. Human muscles work by contracting on one side while the opposing muscle group relaxes. For a simple robot though we could use servo motors. Servos are a rotary motor that could be used to pull or push as needed. While it could be possible to directly couple the motor to allow a one-to-one motion across the joint, coupling the motor to a gear mechanism would greatly increase the torque and strength of the resulting motion, and could be better stabilized. I would be interested in using similiar stepper motors as are commonly found in 3d Printers for all of the motors in my flexible arm as with the right motor driver board these steppers would allow for a high degree of positional accuracy when properly geared for their use, and they also have the ability to detect resistance, meaning that if they are properly calibrated the motors would be able to detect when they begin to encounter resistance. I believe that if calibrated properly it should be possible to pick up an egg with such an arm.

Between 25 seconds to 45 seconds is a wonderful example of a working egg grasping robot arm.

Programming/Circuit Task

  • Since we don’t have all the hardware to build a robotic arm, imagine you have been given the task of creating the elbow joint. Select the appropriate motor for this task, and then create a program and circuit using your Arduino which can demonstrate your motor performing the correct elbow movement. It may help if you tape an object such as a popsicle stick, drinking straw, or long skinny piece of paper to your motor to demonstrate the movement of the lower portion of the arm under control of your program. Your program should take as input a number of degrees to move the elbow from an arbitrary starting position. For example, if you choose ‘fully straight’ as the starting position, this will be designated 0 degrees (start). The arm could then bend about 170 degrees, indicating ‘fully bent’ (check the amount of bend on your own elbow from hand straight out to hand near your shoulder for reference).

I don’t currently have access to a working 3d printer, so my implementation of this project will have to be accomplished using just the parts available in the SIK. Now the two yellow motors included in the kit are supposedly geared at a 48:1 ratio. So I should be able to use one of these as my starting point. Further Circuit 5A is a great place to start as it allows for input from the serial monitor to control a single motor, as such I have begun by returning to that project as my starting point.

The wired up solution

In my solution the main point is to be able to control the rotation of the motor so that we can pass in a degree argument and have the motor move to that angle. Now the first part of this is to pull up the specification on the motor so that we can see the rpm on the motor. The Gearbox is rated for 140RPM unloaded, which works out to 2.33333333 rotations per second. Based on this I then did a test, I set the motor to full power for 0.429 s which should be a rotation of 360 degrees.(I also removed the black cardboard as the yellow motor itself works as an indicator of how far the motor has turned).

A test to see if we can achieve a single rotation.

In my test you can see that there is a bit of overshoot, this is likely due to inertia and the nature of the motor that I am using. 3D printers actually struggle with this as well, and it is possible to write additional software to counter the rotation and inertia so that the motor stops more accurately, but I think that would be overkill for this project. Additionally the countering force needed would change as the load on the joint is adjusted, so if this was in an arm, it would need to be recalibrated for every item the robot picks up. Anyway, I am now able to achieve one full rotation(approximately) in a given time frame, by reducing the active time of the motor motion, I should be able to get close approximations to desired bends.

Now a typical human elbow can bend between 0-145 degrees. Let’s rework the code to allow the user to specify the bend of the elbow and have the motor adjust the joint accordingly.

/*
  Unit 3 Programming/Circuit Task 
  Shawn Ritter
  November 3rd 2021

  references Circuit 5A from the SparkFun Inventor’s Kit
*/

//PIN VARIABLES
//the motor will be controlled by the motor A pins on the motor driver
const int AIN1 = 13;           //control pin 1 on the motor driver for the right motor
const int AIN2 = 12;            //control pin 2 on the motor driver for the right motor
const int PWMA = 11;            //speed control pin on the motor driver for the right motor

//VARIABLES

int currentAngle;       //starting angle for the joint
int desiredAngle;

//CONSTANTS
int MOTORSPEED = 255;       //speed at which the motor should be moved
float FULLROTATION = 428.5714292;     //number of milliseconds for the motor to complete one full revolution
float ROTATION_MODIFIER = 1.08;

void setup() {
  //set the motor control pins as outputs
  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);
  pinMode(PWMA, OUTPUT);

  Serial.begin(9600);                       //begin serial communication with the computer
  Serial.println("Initializing Variables");
  currentAngle = 0;
  desiredAngle = 0;

  Serial.println("Enter joint angle(0-145):");    //Prompt to get input in the serial monitor.
}

void loop() {

        
  while(Serial.available() == 0) {  
  } //wait for input

  Serial.println("Serial Input Received!");
  desiredAngle = Serial.parseInt();     //get the desired angle from the number in the serial message
  //clear the serial buffer
  while(Serial.peek() != -1){
    Serial.read();
  }
  setAngle();
}

int calculateRotation()
{
   int deltaAngle = desiredAngle - currentAngle;
   int rotation = deltaAngle * (ROTATION_MODIFIER * FULLROTATION / 360.0); 
   Serial.print("RotationTime: ");
   Serial.println(rotation);
   return rotation;
}

/********************************************************************************/
void spinMotor(int rotation)                       //function for driving the right motor
{
  if (rotation > 0)                                 //if the motor should drive forward (positive speed)
  {
    digitalWrite(AIN1, HIGH);                         //set pin 1 to high
    digitalWrite(AIN2, LOW);                          //set pin 2 to low
  }
  else if (rotation < 0)                            //if the motor should drive backward (negative speed)
  {
    digitalWrite(AIN1, LOW);                          //set pin 1 to low
    digitalWrite(AIN2, HIGH);                         //set pin 2 to high
  }
  else                                                //if the motor should stop
  {
    digitalWrite(AIN1, LOW);                          //set pin 1 to low
    digitalWrite(AIN2, LOW);                          //set pin 2 to low
  }
  analogWrite(PWMA, MOTORSPEED);                 //now that the motor direction is set, drive it at the entered speed
  delay(abs(rotation));
  analogWrite(PWMA, 0);                 //now that the motor direction is set, drive it at the entered speed
}


void setAngle()
{
  //check if desired Angle valid
  if (desiredAngle >= 0 && desiredAngle <=145)
   {
     Serial.print("CurrentAngle: ");      //print the current Angle info
     Serial.println(currentAngle);

     Serial.print("DesiredAngle: ");
     Serial.println(desiredAngle);
    Serial.println("Acceptable Angle!"); 
    if(desiredAngle != currentAngle)
    {
      spinMotor(calculateRotation());
      currentAngle = desiredAngle;
      desiredAngle = desiredAngle;
    }
   }
   else{
    Serial.println("OOOWWWWWW, Are you trying to break my arm?");
   }
}



And with this code I have a basic elbow motor implementation. There is a bit of overshoot from inertia as my motor is adjusting position, though it would be possible to correct this in a number of ways. One of the easiest ways would be to add a homing switch that could be triggered whenever the joint returns to 0 degrees, this would also allow us to auto home the joint at start up. More precise control and a tighter calibration could also help. Alternatively some inertia compensation logic could also potentially be useful.

Well I think that’s it for this unit,

Shawn Ritter

November 3rd 2021

Leave a Comment

Your email address will not be published. Required fields are marked *

This site uses Akismet to reduce spam. Learn how your comment data is processed.