Cool Robot Leg – TechBuddy

We are going to make a robot leq using an Arduino and 2 micro stepper motors. We will be connecting the two motors together using cool robot attachments.
Schematic

Arduino Code

// Aug 21 2021
// mini servo motor driver
//
//
// This program controls two servo motors 
// Each servo motor has 180 degrees of motion
// The pushbuttons move the motor CW or CCW
//
//
//
// Servo Mtr Wiring
//
//
// Servo Mtr 1
//
// brwn --- GND
// red (middle) -- 3.3V
// org -- pin 6 (PWM)
//
//
// Servo Mtr 2
//
// brwn --- GND
// red (middle) -- 3.3V
// org -- pin 3 (PWM)
//
//
// controller wiring
// 
// Mtr         1            2
// pin      13   12       8     7
//          pb1  pb2     pb3   pb4
// motion   cw   ccw     cw    ccw
//




// This code is for an optional LED Display
//
//
// I2C 128X64 Micro OLED 4-Wire Yellow and Blue Display
// ****************************************************
// **************************************************** 
// WIRING:
// Arduino Pins                                    Micro Oled Pins
//           A4 ---------------------------------- SCL
//           A5 ---------------------------------- SDA
//           3.3V (NOT 5V!!!) -------------------- VCC    (if you use 5V, Then you will get more PUL readings than you should. This is probably because impedence mismatch or too much current draw from the Arduino)
//           GND --------------------------------- GND
//
//
// The first top 16 Pixel rows are yellow (x,y) (x,0) to (x,15)
// The next 48 Pixel rows are blue        (x,y) (x,16) to (x,63)



// INCLUDE LIBRARIES
// *****************
#include <avr/interrupt.h>
#include <SPI.h>
#include <Wire.h>



#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

// The first top 16 Pixel rows are yellow (x,y) (x,0) to (x,15)
// The next 48 Pixel rows are blue        (x,y) (x,16) to (x,63)

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)





// VARIABLE DEFINITIONS
// ********************

int pin_12_13_enabled = 0;
int pin_7_8_enabled = 0;

// initially these values shoud be at a non load-bearing state
int pin_12_13_last_OCR1B = 2000;
int pin_7_8_last_OCR1B = 2000;

// VOLATILE
// used to allow instant changes in ISR

volatile int a = 0;
volatile int b = 0;
volatile int c = 0;

volatile unsigned long CNTR = 0;
unsigned long prior_CNTR = 0;
unsigned long new_CNTR = 0;


volatile int PUL = 0; //PUL from Mach 3
volatile bool DIR = true; //DIR from Mach 3 , Mach 3 sends a high or low voltalge signal to indicate direction , volatile is used to allow instant changes in ISR 

volatile unsigned long CNT = 0; //CNT number of pulses counted from MACH III, volatile is used to allow instant changes in ISR
volatile unsigned long new_CNT = 0;
volatile unsigned long prior_CNT = 0;

volatile bool HA = false; //HA hall a from SERVO II
volatile bool HB = false; //HB hall b from SERVO II
volatile bool HI = false; //HI hall i from SERVO II
 







// START void setup() 
// ******************************************************************************************************************************************
void setup()
{
//DDRD = B00000000; //  set pin 3,5,6 as OUTPUTS

  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally

//DDRD = B00000000; //  set pin 3,5,6 as OUTPUTS

//CLEAR THE ADAFRUIT LOGO



// Pin 7 
pinMode(7, INPUT);           // set pin A0 to input
digitalWrite(7, HIGH);       // turn on pullup resistors on pin A0

// Pin 8 
pinMode(8, INPUT);           // set pin A0 to input
digitalWrite(8, HIGH);       // turn on pullup resistors on pin A0

// Pin 12
pinMode(12, INPUT);           // set pin 12 to input for OCR1B--
digitalWrite(12, HIGH);       // turn on pullup resistors on pin 12

// Pin 13
pinMode(13, INPUT);           // set pin 13 to input for OCR1B++
digitalWrite(13, HIGH);       // turn on pullup resistors on pin 13



// DIR from Mach 3 --> ARDUINO
// HA, HB from SERVO II Motor
// this is for PCINT0-PCINT23
// **************************
cli(); // stop interrupts
//PCICR |= 0b00000111; // Enable Ports B, C, and D pin change interrupts
//PCICR |= 0b00000001; // Enable Ports B pin change interrupts
PCICR |= (1<<PCIE0);//enable group interrupts on PORTB PCINT[7:0]
//PCMSK0 |= 0b00000010; // DIR -> PCINT4 -> printed_pin_12
//PCMSK0 |= (1<<PCINT4);// DIR -> PCINT4 -> printed_pin_12
//PCMSK0 |= (1 << PCINT4);  //Set PCINT0 (digital input 12) to trigger an interrupt on state change.
PCMSK0 |= (1<<PCINT4);//enable interrupt pin 12
//PCMSK1 |= 0b00000001; // HA ->  PCINT13 -> printed_pin_A0
//PCMSK2 |= 0b10000000; // HB ->  PCINT16 -> printed_pin_0
sei(); // start interrupts



// PUL from MachIII --> UC100-pin3 --> ARDUINO-pin2
// this is for INT0 = printed_pin_2
// UC100 pin is 3 the Arduino pin is 2
// ********************************
 attachInterrupt (digitalPinToInterrupt (2), pulse, FALLING);  // attach interrupt handler
// Serial.begin(9600);
 

// ARDUINO(pins3,5,6)5vOUTPUT --> MOSFETs(A,B,C)GATES --> SERVOII_Motor_Coils(A,B,C) 
// *********************************************************************************
// The arduino will send 5V to the MOSFET gates by changing the DDRD ports 3,5,6 from 0 to 1


// PWM Duty Cycle
// **************
// 20 - 255 :  10% - 100%
// OC0A is pin 6 is PhA
// OC0B is pin 5 is PhB
// OC2B is pin 3 is PhC
// 100 good for x & y axis using 24 v ,130-140 good for z axis using 24v, 
// 40 is good for z axis using 120v about 5 bumps per inch of travel
// 20 is better at 240volts 2 bumps per inch of travel
// note 3/31/2021: z axis has jumpy spots even using 120 volts. the wheel 
// feels looser after lostening nuts on saddle that gous up and down and adding lube. 
// I will probably mess with the OCRB stuffs
OCR0A = 255;  
OCR0B = 255;  
OCR2B = 255;  

// Timer/Counter0
// *************
// pin5 & pin6
TCCR0A = _BV(COM0A1) | _BV(COM0B1) | _BV(WGM01) | _BV(WGM00); // Set bit to 1 at these locations
// set to clkio/64 to give 976.56Hz
//TCCR0B &= ~(1 << CS02); // set bit to 0
//TCCR0B |= (1 << CS01); // set bit to 1
//TCCR0B |= (1 << CS00); // set bit to 1

// set to clkio/1024 to give ???hz
TCCR0B |= (1 << CS02); // set bit to 1
TCCR0B &= ~(1 << CS01); // set bit to 0
TCCR0B &= ~(1 << CS00); // set bit to 0

// Timer/Counter2
// *****************
// pin3 & pin11
TCCR2A = _BV(COM2A1) | _BV(COM2B1) | _BV(WGM21) | _BV(WGM20); // Set bit to 1 where specified
// set to clkio/64 to give 980.39Hz
//TCCR2B |= (1 << CS22); // set bit to 1
//TCCR2B &= ~(1 << CS21); // set bit to 0
//TCCR2B &= ~(1 << CS20); // set bit to 0


// set to clkio/1024 to give ??HZ 
TCCR2B |= (1 << CS22); // set bit to 1
TCCR2B |= (1 << CS21); // set bit to 1
TCCR2B &= ~(1 << CS20); // set bit to 0

// ALIGN MOTORS TO PHASE A
// ***********************
// phase A of Servo II corresponds to Arduino Digital Pin #6
/*
  DDRD |= (1 << DDD3); //////////////////////////////////////////////////6 on
  a = 1;
  b = 1;
  delay(50);
  DDRD &= ~(1 << DDD3); //////////////////////////////////////////////////6 off
  delay(100);
  
  DDRD |= (1 << DDD3); //////////////////////////////////////////////////6 on
  a = 1;
  b = 1;
  delay(50);
  DDRD &= ~(1 << DDD3); //////////////////////////////////////////////////6 off
  delay(100);

  DDRD |= (1 << DDD3); //////////////////////////////////////////////////6 on
  a = 1;
  b = 1;
  delay(50);
  DDRD &= ~(1 << DDD3); //////////////////////////////////////////////////6 off
  delay(100);
  /*
  DDRD |= (1 << DDD5); ///////////////////////////////////////////////// 5 on
  a = 1;
  b = 1;
  delay(100);
  DDRD &= ~(1 << DDD5); ///////////////////////////////////////////////// 5 off
  delay(100);
  DDRD |= (1 << DDD3); ///////////////////////////////////////////////// 3 on
  a = 1;
  b = 1;
  delay(100);
  DDRD &= ~(1 << DDD3); ///////////////////////////////////////////////// 3 off
*/

cli(); // stop interrupts

// Timer/Counter1
// *************
// This timer is in charge of triggering the SERVO II motor phase A, B, and C on and off
//set timer1 interrupt
  TCCR1A = 0;// set entire TCCR1A register to 0
  TCCR1B = 0;// same for TCCR1B
  TCNT1  = 0;//initialize counter value to 0
  // set compare match register for 1hz increments
  // OCR1A must be < 65536 
  // t_on = OCR1B
  // t_off = OCR1A - OCR1B
  //OCR1A = 7875;  // TEST ZONE works good on X-axis 
  OCR1A = 40000;  // value of 30,  z-axis -- as of Mar-2-2021 WORKS AWESOME on Z-axis:  1680 steps per inch, 1.5 in per min using 120 volts, 100 accel, 
 // ocr1a();
    
  // set compare match register for 1hz increments
  // OCR1B must be < 65536 
  // t_on = OCR1B
  // t_off = OCR1A - OCR1B
  // OCR1B = 6250;  // TEST ZONE
  OCR1B = 4000;  // 700 to 5200 for MG90S
  //ocr1b(); 
  
  // turn on CTC mode
  TCCR1B |= (1 << WGM12);
  // Set CS12 bit for 8 prescaler
  TCCR1B &= ~(1 << CS12) ; // set to 0
  TCCR1B |= (1 << CS11) ; // set to 1
  TCCR1B &= ~(1 << CS10) ; // set to 0
  // enable timer compare interrupt
  TIMSK1 |= (1 << OCIE1A); // OCR1A
  // enable timer compare interrupt
  TIMSK1 |= (1 << OCIE1B); // OCR1B

sei(); // start interrupts

}
// ***********************************************************************************************************************************************************
// END void setup()






// PCINT0
//  ISR ISR ISR ISR ISR ISR ISR ISR ISR
// (ISR) Interrupt Service Routine (ISR)
//  DIR -> printed_pin_12 
//  ************************************
//Serial.println("Inside ISR(PCINT0_vect)");
//DIR -> printed_pin_12 
//+DIR -> printed_pin_12 is HIGH --> boolean true
//-DIR -> printed_pin_12 is LOW  --> boolean false
//int DIR = true default value at start-up
//int new_DIR = 0; default value at start-up
//int prior_DIR = 0; default value at start-up
// the pin on UC100 is 2 and the pin on Arduino is 12
ISR(PCINT0_vect)
{
 // DIR = !DIR;
 // if(PINB & (1 << 4))   // if pin 12 -> DIR is 5V
 // Serial.println(DIR);
} 


// PCINT1
//  ISR ISR ISR ISR ISR ISR ISR ISR ISR
// (ISR) Interrupt Service Routine (ISR)
//  HA -> printed_pin_A0
// *************************************
//ISR(PCINT1_vect)
//{
// HA -> printed_pin_A0
//} 


//  ISR ISR ISR ISR ISR ISR ISR ISR ISR
// (ISR) Interrupt Service Routine (ISR)
//  HB -> printed_pin_A0
// *************************************
//ISR(PCINT2_vect)
//{
// HB
//} 


//  ISR ISR ISR ISR ISR ISR ISR ISR ISR
// (ISR) Interrupt Service Routine (ISR)
// *************************************
//pulse_counter ++; inside voide pulse()
void pulse()
{
  CNTR++;
  CNT++;
} 


// FUNCTION
// ********
void read_write_inputs()
{
  // MACH 3 STEP PULSES sent to SERIAL MONITOR
  new_CNT = CNT;
  if (new_CNT != prior_CNT) 
  {
       
    //Serial.println("Inside void read_write_inputs()") 
    //Serial.print("CNT: ");
    //Serial.println(new_CNT);
    prior_CNT = new_CNT;
    //delay(10); // print at reasonable speed
  }
  
  // MACH 3 DIR sent to SERIAL MONITOR
  if(PINB & (1 << 4))   // if pin12/DIR is 5V
  {
   DIR = 1;
  }
  else
  {
   DIR = 0;
  }

  
}













// 
// -------------------------------
void ocr1a(void) 
{
    OCR1A = 40000;
}


// 
// -------------------------------
void ocr1b(void) 
{
//700 - 5200
    OCR1B = 2000 ;
}







//  TIMER 1 COMPA OCR1A
// (ISR) Interrupt Service Routine (ISR)
//  ************************************
ISR(TIMER1_COMPA_vect)
{
  if(pin_12_13_enabled == 1)
  {
  DDRD |= (1 << DDD6);  ///////////////////////////////////////////////// 6 on
  }

  else if(pin_7_8_enabled == 1)
  {
  DDRD |= (1 << DDD3);  ///////////////////////////////////////////////// 3 on
  }
}
// END ISR(Timer1_COMPB_vect)


//  TIMER 1 COMPB OCR1B
// (ISR) Interrupt Service Routine (ISR)
//  ************************************
ISR(TIMER1_COMPB_vect)
{
  if(pin_12_13_enabled == 1)
  {
  DDRD &= ~(1 << DDD6);  ///////////////////////////////////////////////// 6 off
  }
  
  else if(pin_7_8_enabled == 1)
  {
  DDRD &= ~(1 << DDD3);  ///////////////////////////////////////////////// 3 off 
  }
  
}
// END ISR(Timer1_COMPA_vect)




// MAIN LOOP MAIN LOOP MAIN LOOP MAIN LOOP MAIN LOOP MAIN LOOP MAIN LOOP MAIN LOOP MAIN LOOP MAIN LOOP MAIN LOOP MAIN LOOP MAIN LOOP MAIN LOOP
// ********* ********* ********* ********* ********* ********* ********* ********* ********* ********* ********* ********* ********* *********
void loop()
{
//read_write_inputs();       // Arduino to Serial Monitor
//display_HMI_text();        // Arduino to HMI

// Check the state of input pin 12 from the push button

if (!digitalRead(12))
{

OCR1B = pin_12_13_last_OCR1B;

pin_12_13_enabled = 1;
pin_7_8_enabled = 0;
 

  if (OCR1B > 700)
  {
    delay(10);
    OCR1B = OCR1B - 25;
    pin_12_13_last_OCR1B = OCR1B;
  }
}


// Check the state of input pin 13 from the push button
else if (!digitalRead(13))
{
  
OCR1B = pin_12_13_last_OCR1B;

pin_12_13_enabled = 1;
pin_7_8_enabled = 0;
  
  if (OCR1B < 4500)
  {
    delay(10);
    OCR1B = OCR1B + 25;
    pin_12_13_last_OCR1B = OCR1B;
  }
}




else if (!digitalRead(7))
{

OCR1B = pin_7_8_last_OCR1B;

pin_7_8_enabled = 1;
pin_12_13_enabled = 0; 


  if (OCR1B >= 700)
  {
    delay(10);
    OCR1B = OCR1B - 25;
    pin_7_8_last_OCR1B = OCR1B;
  }
}


// Check the state of input pin 8 from the push button
else if (!digitalRead(8))
{

OCR1B = pin_7_8_last_OCR1B;

pin_7_8_enabled = 1;
pin_12_13_enabled = 0;

  if (OCR1B <= 5200)
  {
    delay(10);
    OCR1B = OCR1B + 25;
    pin_7_8_last_OCR1B = OCR1B;
  }
}






//  new_CNTR = CNTR;
//  if (new_CNTR != prior_CNTR) 
//  {
//    Serial.println(new_CNTR);
//    prior_CNTR = new_CNTR;
//    delay(10); // print at reasonable speed
//  }


}
// MAIN LOOP MAIN LOOP MAIN LOOP MAIN LOOP MAIN LOOP MAIN LOOP MAIN LOOP MAIN LOOP MAIN LOOP MAIN LOOP MAIN LOOP MAIN LOOP MAIN LOOP MAIN LOOP
// ********* ********* ********* ********* ********* ********* ********* ********* ********* ********* ********* ********* ********* *********