Get a motor spinning

[CalebJ2]

GanttStart: 2019-09-26 GanttDue: 2020-01-25

Write a simple arduino sketch to control the motor driver and spin a motor.

• [ ] Wire up the arduino, driver, and motor
• [ ] Write an arduino sketch
• [ ] Run it

[CalebJ2]

Created another repository for the Arduino code because it seems like a separate thing. We could add it as a git submodule later if we want the code in this repo. https://github.com/WWU-Robotics-Club/ROS-motor-control

I think we can make it a rosserial node: http://wiki.ros.org/rosserial http://wiki.ros.org/rosserial_arduino/Tutorials

[CalebJ2]

Got a motor running using a TB6612 ( https://www.aliexpress.com/item/32903346167.html ) and this code:

int speed = 0;
bool increasing = true;

void setup() {
    pinMode(1, OUTPUT);
    pinMode(2, OUTPUT);
    pinMode(3, OUTPUT);
    digitalWrite(1, HIGH);
    digitalWrite(2, LOW);
}

void loop() {
    // put your main code here, to run repeatedly:
    if (increasing)
    {
        speed += 5;
        if (speed > 250)
        {
        increasing = false;
        digitalWrite(1, LOW);
        digitalWrite(2, HIGH);
        }
    }
    else
    {
        speed -= 5;
        if (speed < -250)
        {
        increasing = true;
        digitalWrite(1, HIGH);
        digitalWrite(2, LOW);
        }
    }

    analogWrite(3, abs(speed));
    delay(30);
}

[CalebJ2]

Motor with encoder and PID:

// https://www.pjrc.com/teensy/td_libs_Encoder.html
#include <Encoder.h>

Encoder encoder(11,12);
double currentPos = 0;
double previousPos = 0;
float currentError = 0;
float previousError = 0;
double previousTime = 0;

int stepsPerRev = 1435;

float kp = 90;
float ki = 100;
float kd = 5;

float errorIntegral = 0;
float targetSpeed = 0.8;

void setup() {
    Serial.begin(9600);
    Serial.println("Encoder Test:");
    // set up motor pins
    pinMode(1, OUTPUT);
    pinMode(2, OUTPUT);
    pinMode(3, OUTPUT);
    // enable going one direction
    digitalWrite(1, HIGH);
    digitalWrite(2, LOW);
}

void loop() {
    // put your main code here, to run repeatedly:
    previousPos = currentPos;
    currentPos = encoder.read();
    float dPos = (float)(currentPos-previousPos)/stepsPerRev; // revolutions

    double currentTime = millis();
    float dT = (currentTime-previousTime)/1000.0; // seconds
    previousTime = currentTime;

    float speed = dPos/dT;

    previousError = currentError;
    currentError = speed - targetSpeed;

    errorIntegral += currentError * dT;
    float errorDerivative = (currentError - previousError)/dT;

    float output = targetSpeed * kp - errorIntegral * ki - errorDerivative * kd;
    writeSpeed(output);
    Serial.println(String(speed) + "," + currentPos + "," + String(errorIntegral));
    if (Serial.available()) {
        Serial.read();
        Serial.println("Reset to zero");
        encoder.write(0);
    }
    delay(10);
}

void writeSpeed(float speed) {
    if (speed > 0) 
    {
        digitalWrite(1, LOW);
        digitalWrite(2, HIGH);
    }
    else {
        digitalWrite(1, HIGH);
        digitalWrite(2, LOW);
    }
    analogWrite(3, constrain(abs(speed),0,255));
}