Electronics:

Elegoo UNO R3 Microcontroller

Greartisan DC 12V 120RPM Gear Motor 

L298N Motor Driver

12 V Power Supply

12 V 1A DC Adapter

Adafruit LIS3DH Triple-Axis Accelerometer

Electronics:

Process on Iterating Code

The original idea to control this motor was to collect the x and y acceleration data from the IMU. Using these values, we would use inverse tangent to find the angle of link 3 and slow the motors down once we reached the desired orientation. However, this only works when the links are stationary. If the links are moving, then the x and y accelerations would change due to the angular velocity of the links. If we do the inverse tangent of the x and y accelerations, we would get faulty orientations. Therefore, we switched to only measuring the x acceleration of link 3. If the x acceleration of link 3 is equal to gravity, this means that link 3 is vertical. If the acceleration is 0, it means that the link is horizontal. If the acceleration is negative, it means that the link is in the pouring stage. The change in acceleration due to the moving links would have a minimal effect compared to the first method. Using these metrics, we tried to control the motor so that when link 3 was vertical, we would increase the speed to help the cup fight against gravity. We would then slow down the cup when the x acceleration started to approach 0 because the cups no longer need extra power to fight gravity as much. When the acceleration reached 0, we would reverse direction because at this point the inertia of the cups would result in the links slamming into the backboard. However, the motors need a certain speed to fight against both stiction and gravity of the links. By the time the Arduino calculated and sent the instructions to slow down the motor, the links and cups would already slam against the backboard of the mechanism. Therefore, our final code resulted in us detecting when link 3 reaches the pouring phase, and then switching directions. We no longer considered slowing the motor down. The final code is shown below.

Arduino Code:

#include <Wire.h>              // Must include Wire library for I2C
#include <SparkFunLSM6DSO.h> //
#include <SPI.h>
#include <Adafruit_LIS3DH.h>
#include <Adafruit_Sensor.h>

#define enA 9
#define in1 4
#define in2 5
// Used for software SPI
#define LIS3DH_CLK 13
#define LIS3DH_MISO 12
#define LIS3DH_MOSI 11
// Used for hardware & software SPI
#define LIS3DH_CS 10

// software SPI
//Adafruit_LIS3DH lis = Adafruit_LIS3DH(LIS3DH_CS, LIS3DH_MOSI, LIS3DH_MISO, LIS3DH_CLK);
// hardware SPI
//Adafruit_LIS3DH lis = Adafruit_LIS3DH(LIS3DH_CS);
// I2C
Adafruit_LIS3DH lis = Adafruit_LIS3DH();
Adafruit_LIS3DH second=Adafruit_LIS3DH();


unsigned long timer = 0;    // used to check current time [microseconds]
long loopTime = 0;       // default time between updates, but will be set in python Code [microseconds]
bool initLooptime = false;  // boolean (T/F) to check if loop time has already been set
bool stopProgram = false;

int rawAccX1 = 0;
int rawAccY1 = 0;
int rawAccX2 = 0;
int rawAccY2=0;

int rawGyroX = 0;
int rawGyroY = 0;
int rawGyroZ = 0;

// when true, the left side starts to turn anticlockwise, pours
bool clockwise=true;

int theta1=0;
int theta2=0;
int thetas=0;
int sensi=1;

int motorSpeedA = 0;
int u=0;
int speed=0;

void setup() {
  pinMode(enA, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  Serial.begin(115200);
  while (!Serial) delay(10);     // will pause Zero, Leonardo, etc until serial console opens

  if (! lis.begin(0x18)) {   // change this to 0x19 for alternative i2c address
    Serial.println("Couldnt start");
    while (1) yield();
  }
  if (! second.begin(0x19)) {   // change this to 0x19 for alternative i2c address
    Serial.println("Couldnt start");
    while (1) yield();
  }

  // lis.setRange(LIS3DH_RANGE_4_G);   // 2, 4, 8 or 16 G!

  Serial.print("Range = "); Serial.print(2 << lis.getRange());
  Serial.println("G");

  // lis.setDataRate(LIS3DH_DATARATE_50_HZ);
  Serial.print("Data rate set to: ");
  switch (lis.getDataRate()) {
    case LIS3DH_DATARATE_1_HZ: Serial.println("1 Hz"); break;
    case LIS3DH_DATARATE_10_HZ: Serial.println("10 Hz"); break;
    case LIS3DH_DATARATE_25_HZ: Serial.println("25 Hz"); break;
    case LIS3DH_DATARATE_50_HZ: Serial.println("50 Hz"); break;
    case LIS3DH_DATARATE_100_HZ: Serial.println("100 Hz"); break;
    case LIS3DH_DATARATE_200_HZ: Serial.println("200 Hz"); break;
    case LIS3DH_DATARATE_400_HZ: Serial.println("400 Hz"); break;

    case LIS3DH_DATARATE_POWERDOWN: Serial.println("Powered Down"); break;
    case LIS3DH_DATARATE_LOWPOWER_5KHZ: Serial.println("5 Khz Low Power"); break;
    case LIS3DH_DATARATE_LOWPOWER_1K6HZ: Serial.println("16 Khz Low Power"); break;
  }      
}

void loop() {
  // put your main code here, to run repeatedly:
  int yAxis = analogRead(A1); // Read Joysticks Y-axis
  if (Serial.available() > 0) {       // if data is available
    //String str = Serial.readStringUntil('\n');

    readFromJoystick(yAxis);
  }
  lis.read();      // get X Y and Z data at once
  sensors_event_t event;
  lis.getEvent(&event);
  second.read();
  sensors_event_t event1;
  second.getEvent(&event1);
  rawAccX1=event.acceleration.x;
  rawAccY1=event.acceleration.y;
  rawAccX2=event1.acceleration.x;
  rawAccY2=event1.acceleration.y;
  // right side is rising (IMU back is facing the front)
  if (clockwise==true)
  {
    if ((rawAccX1-4)<(0+sensi) && (rawAccX1-4)>(0-sensi))
    {
      clockwise=!clockwise;
    }
    else
    {
      theMotor(clockwise,200);
    }
  }
  // left side is rising
  if (clockwise==false)
  {
    if ((rawAccX2-4<0+sensi) && (rawAccX2-4>0-sensi))
    {
      clockwise=!clockwise;
    }
    else
    {
      theMotor(clockwise,200);
    }
  }
}

//if forward true, motor moves forward, speed in range (0,255)
void theMotor(bool forward, int speed){
  // Set Motor A forward
  if (forward==true){
    digitalWrite(in1, LOW);
    digitalWrite(in2, HIGH);
  }
  if (forward==false){
    digitalWrite(in1, HIGH);
    digitalWrite(in2, LOW);
  }
  motorSpeedA=speed;
  analogWrite(enA, motorSpeedA); // Send PWM signal to motor A
}

void stop()
{
  theMotor(true,0);
}

void slowdown(bool dir,int x)
{
  for (int i=0; i<(x-100);i++)
  {
    theMotor(dir,x-i);
    delay(50);
  }
}

void rampup(bool dir, int x)
{
  for (int i=x;i<200;i++)
  {
    theMotor(dir,i);
    delay(50);
  }
}


void readFromJoystick(int input)
{
  char command = input;  
  int rate = 0;
  if (input>700)
  {
    // stop command
    stopProgram = true;
  }
  else if (input<300)
  {
    // free run
    initLooptime = true;
    stopProgram = true;
  }

}