Reef Angel Forum

Code: Select all

// stepper motor receive code from the RA 
// designed to input a position from the serial integer input then go to it on the dolly

#include <Wire.h>


int stp = 13;  //connect pin 13 to step
int dir = 12;  // connect pin 12 to dir
int enb = 11;  //enable pin for motor sheild
int ledred = 10;  //led poweron
int ledgreen = 9;  //led moving forward
int ledyellow = 8;  //led moving backward
int pos = 0;     //  gen counter
int newpos = 0;     // last request for a position
long dis = 0;     // calculated distance
long dist =0;
String readString;
int endx = 9500;   //end of the line
char rx_byte = 0;
String rx_str = "";
boolean not_number = false;


void setup() 
{
  Wire.begin(11); // I2C address 1q
  Wire.onReceive(receiveEvent);  
  Serial.begin(9600);
  pinMode(stp, OUTPUT);
  pinMode(dir, OUTPUT);
  pinMode(enb, OUTPUT);
  pinMode(ledred, OUTPUT);
  pinMode(ledgreen, OUTPUT);
  pinMode(ledyellow, OUTPUT);
  digitalWrite(ledred, HIGH);
  digitalWrite(ledyellow, LOW);
  digitalWrite(ledgreen, LOW);

}


void loop() 
{

//  Readint();
  MoveCam();
}

void MoveCam()
{
  digitalWrite(enb,HIGH);
  if (newpos != pos) 
   {
     digitalWrite(enb,LOW);
     if (newpos > pos)
     { 
      dis = newpos-pos;
      if (newpos>endx) 
      {
       digitalWrite(ledgreen, HIGH);
       digitalWrite(ledyellow, HIGH);
       newpos = 9500;
      }
      else
      {
//      Serial.print("Distance forward: ");
//      Serial.println(dis);
//      Serial.print ("current position   ");
//      Serial.println (pos);
//      Serial.print ("new position   ");
//      Serial.println (newpos);
      digitalWrite (dir, HIGH);    
      dist = dis*10;  
      for (int i=0; i < dist; i++)
      {
       digitalWrite(ledgreen, HIGH);
       digitalWrite(stp, HIGH);   
       delayMicroseconds(100);               
       digitalWrite(stp, LOW);  
       delayMicroseconds(100); 
      }
      digitalWrite(ledgreen, LOW);
//      Serial.print("Current Position from: ");
//      Serial.print(pos);
      pos=newpos;  //set the current position for comparison next time
//      Serial.print("   to    ");
//     Serial.println(pos);
      digitalWrite(enb, HIGH);
     }
   }
   else if (newpos < pos)
   {
      dis = pos-newpos;  
//      Serial.print("Distance backward: ");
//      Serial.println(dis);
      dist = dis*10;  
      digitalWrite (dir, LOW);      
      for (int i=0; i < dist; i++)
      {
       digitalWrite(ledyellow, HIGH);
       digitalWrite(stp, HIGH);   
       delayMicroseconds(100);               
       digitalWrite(stp, LOW); 
       delayMicroseconds(100); 
      }
      digitalWrite(ledyellow, LOW);
//      Serial.print("Current Position from: ");
//      Serial.print(pos);
      pos =newpos;  //set the current position for comparison next time
//     Serial.print("   to    ");
//      Serial.println(pos); 
      digitalWrite(enb, HIGH);    
     }
   }
}
  


void Readint()
{
  if (Serial.available() > 0) {    // is a character available?
    rx_byte = Serial.read();       // get the character
    if (rx_byte == 'p') {
      Serial.print ("current position   ");
      Serial.println (pos);
      Serial.print ("new position   ");
      Serial.println (newpos);
      Serial.print ("distance   ");
      Serial.println (dis);
      not_number = true;    // flag a non-number
    }    
    else if (rx_byte == '\n') {
      rx_str = "";                // clear the string for reuse
   }
  } 
}


void receiveEvent(int howMany) 
{
  if (howMany==5)  // Our custom protocol is 13 bytes
  {
    byte cmd1,cmd2,cmd3,cmd4,cmd5;

    cmd1=Wire.read();
    cmd2=Wire.read();
    cmd3=Wire.read();
    cmd4=Wire.read();
    cmd5=Wire.read();

    if (cmd1=='%' && cmd2=='%' && cmd3=='%') // the first 3 bytes of the custom protocol are $$$ to ensure it's coming from RA
    {
      newpos=(cmd4<<8)+cmd5;
    }
  }
  else
    for (int a=0;a<howMany;a++)
      Wire.read(); // if the number of bytes is not 32, discard everything
}