3DoT Firmware:(PID 180^o Move Control)

By Kyle Gee and Ryan Vega:

To preface this section, it is important to note that a majority of the code utilizes Brandon Nguyen’s code for the Hexy as a base. I translated the code to work with a 3DoT board because the original code was for an Arduino Nano. I also set up an interrupt-service routine (ISR) for the code.

// Pinouts for ATmega32U4
// Motor Driver Definitions
#define AIN1 12
#define AIN2 4
#define PWMA 6
#define BIN1 9
#define BIN2 5
#define PWMB 10
#define STBY 8

// Shaft Encoder Definitions (These need external interrupt pins) 
#define outAR 16 // Right Motor Output
#define outAL 17 // Left Motor Output

// Gear Position Sensor
#define phaR 15 // Right Gear 
#define phaL 14 // Left Gear works

uint8_t gearLeft, gearRight, pastGearLeft, pastGearRight;
uint8_t setupTickL, setupTickR;


// Phase Variables
float totalTicks= 720;
float phaseRight, phaseLeft, phaseError;

int rightPWM = 180; 
int leftPWM = 180; //original 180
int maxRightPWM = 200;
int maxLeftPWM = 200;
int minRightPWM = 160;
int minLeftPWM = 160;

volatile long ticksR, ticksL; 
volatile uint8_t pinLast = 0;

// --------------- PID Controller --------------- 
double SetpointR, InputR, OutputR;
//Specify the links and initial tuning parameters | P,I,D = 2,6,1
PID myPID_R(&InputR, &OutputR, &SetpointR, 2, 6, 1, DIRECT);

//uint8_t dpMaxSteps = 128; //remember even thought the the digital pot has 128 steps it looses one on either end (usually cant go all the way to last tick)
//int maxRangeOhms = 100000; //this is a 5K potentiometer
//MCP4017 i2cDP(MCP4017ADDRESS, dpMaxSteps, maxRangeOhms);

//ISR
ISR(PCINT0_vect){
uint8_t pinStatus= PINB & (_BV(PB2)|_BV(PB0));
uint8_t pinChange= pinLast^pinStatus;
pinLast=pinStatus;
if(pinChange==_BV(PB0))ticksR++;
if(pinChange==_BV(PB2))ticksL++;

}

Functions used to setup 180^o out of phase and to keep in phase:

void countTicksR() {
ticksR++;
}

void countTicksL() {
ticksL++;
}

void Init3DoT() { 
// Motor Driver = OUTPUT

pinMode(AIN1,OUTPUT);
pinMode(AIN2,OUTPUT);
pinMode(PWMA,OUTPUT);
pinMode(STBY,OUTPUT);
pinMode(BIN1,OUTPUT);
pinMode(BIN2,OUTPUT);
pinMode(PWMB,OUTPUT);
pinMode(BLINK, OUTPUT);
/*
// Ultrasonic Sensors
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
*/
//Absolute phase encoder
pinMode(phaR, INPUT_PULLUP);
pinMode(phaL, INPUT_PULLUP);

Serial.begin(9600);
}

void left_forward(){
digitalWrite(AIN1,LOW);
digitalWrite(AIN2,HIGH);
analogWrite(PWMA,leftPWM);
digitalWrite(STBY,HIGH);
}

void left_stop(){
digitalWrite(AIN1,LOW);
digitalWrite(AIN2,LOW);
analogWrite(PWMA,leftPWM);
digitalWrite(STBY,HIGH); //gearRight = digitalRead(phaR);
//Serial.println(phaR);
}
void right_forward(){
digitalWrite(BIN1,LOW);
digitalWrite(BIN2,HIGH);
analogWrite(PWMB,rightPWM);
digitalWrite(STBY,HIGH);
}

void right_stop(){
digitalWrite(BIN1,LOW);
digitalWrite(BIN2,LOW);
analogWrite(PWMB,rightPWM);
digitalWrite(STBY,HIGH);

}

In the beginning of the code are just the definitions and the libraries used for the 180^o out of phase motors. Before the setup, I create the process that will be done when the ISR is called. In the ISR, I take the current pin readings and it to PB2 and PB0 this clears the other values leaving just the information at the 2 pins. Then it does an exclusive, or to see which pin actually changed and sets the new pin’s last status. Lastly it does 2 “if” statements to determine which tick variable to increment. Then in the setup, I enSoftable the pins for pin-change interrupts, then locally enable interrupts, and lastly globally enable the interrupt.

Within setup it moves the left motor till it receives a reading from the hall effect sensor. Once the left motor has stopped, it then moves the right motor until it achieves the 180^o out-of-phase at the start. In the loop, the code keeps checking the amount of times the shaft encoders tick, which is held by the left and right tick variable that is counted in the ISR.  Based on the readind it will make the appropriate adjustments to get the motor back aligned.

Recognition Software:

U8GLIB_SSD1306_128X64 u8g(U8G_I2C_OPT_NO_ACK); // Display which does not send AC

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

//////////// end of Roshandra code
//////////////  Ryan's modified ArxRobot_Telecom code
#define DETECTION    0x40 // Sets the hex value that the ArxRobot APP will send for the Detection custom command
#define RPiOFF       0x41 // Sets the hex value that the ArxRobot APP will send for the RPiOFF custom command
#define SCREEN_WIDTH 128  // OLED display width, in pixels
#define SCREEN_HEIGHT 64  // OLED display height, in pixels
const uint8_t CMD_LIST_SIZE = 2;    // we are adding 2 commands (DETECTION, RPiOFF)

int byteIn = '0';                   //this variable will be used for information sent serially between the RPi and the 3dot
// The following setion was borrowed from the ArxRobot_Telecom file found in the ArxRobot library

// and modified to create the two custom commands

ArxRobot::cmdFunc_t onCommand[CMD_LIST_SIZE] = {{DETECTION,detectionHandler}, {RPiOFF,rpiOffHandler}};

// This is the end of the custom command code used to send the ArxRobot APP command from the 3dot Board to the RPi via USB

ArxRobot ArxRobot;                  // make a 3DoT Robot

In Setup:

Serial.begin(9600);             // default = 115200

    ArxRobot.begin();               // initialize hardware

    ArxRobot.setCurrentLimit(60);   // set current limit to 560mA

                                    // see https://www.arxterra.com/current-limit/

 /////////// Roshandra

 

  ArxRobot.setOnCommand(onCommand, CMD_LIST_SIZE);

    if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { 

    // Address 0x3D for 128x64

    //delay(2000);

    //Serial.println(F("SSD1306 allocation failed"));

///////////////// end of Roshandra

  }

  ////////////  Ryan's code

  // This code will run when the 3 dot board is first turned on.

  // The OLED will display a Greeting, reminder to start the RPi, and will prompt you to turn the RPi detection on(which is done by the APP). 
  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.setCursor(0, 10);
  display.println("HELLO, I'm\n\nMicroSPOT");  
  display.display();
  delay(5000);
  display.clearDisplay();
  display.setCursor(0, 10);
  display.println("Turn RPi\n   ON");
  display.display();
  delay(5000);
  display.clearDisplay();
  display.setCursor(0, 10);
  display.println("Start RPi\nDetection");
  display.display();
  delay(5000);
////////////////////////////////

In Loop:

Functions:

While a serial connection is established between the Raspberry Pi (RPi) and the 3DoT board via the USB; the code at the end of the main loop, the “while (Serial.available () > 0)” code, will determine if there is any information stored in the serial buffer. If the Raspberry Pi detects an image it will send a certain value to the 3DoT board. The 3DoT board will store that value in the variable “byteIn” when the Serial.read() function is called. Next the “Draw” function will be called from the main loop.  Based on the value of “byteIn” that gets sent to the 3DoT board by the RPi the draw function will display an image on the 3DoT board’s OLED. In addition, the draw function changes the motors’ settings based on the value of the “byteIn.”