Hopper Bot

///Note:each Servo is a little different. Reed switch may be off bc of placing. <-quickest fix would be to change the if limit distance
//pin3 is servo
//pin2 is reed switch
//pin13 is piston
//A1 is pot for mode 
//A0 is pot for L/R  
//A4 and A5 is Mag          

////////////////////////////////////////////
long interval =  .125*1000; //once every .125 seconds for PISTON
long servointerval = 500; //once every 1 seconds for SERVO
long reedinterval=10;



///////////////////////////////////////////reed switch
int reedSwitch = 2; //the reedswitch is pin 2
long readReedSwitch = 0; //a variable(that will change once wheels/magnets are moving)
long CurrentreadReedSwitch = 0; // current variable(that will change once wheels/magnets are moving)
long oldreadReedSwitch = 0; //an old variable(that will change once wheels/magnets are moving)
long CurrentCurrentreadReedSwitch = 0; //The variable shown on the monitor
int Wall = 0; //to prevent double counting
int PastWall = 0; //to count only when 0 changes to 1 or 1 changes to 0
int distance = 0; //total distance. Including the mode distances
long PreviousOnMilliR =0;
//////////////////////////////////////////

////////////piston starts
int piston = 13; //pin13 is piston
int state = LOW; // the state is low
long PreviousOnMilliP = 0; //will store last time piston was on
////////////piston end

//////////////////////////////////////////pot for mode
int pot = A1;    // A1 input pin for the potentiometer for mode
int val = 0;     //value for input of pot
int number =0;   //what mode it is in
int test=0;      //This will be the distance it is away from the true starting point
//////////////////////////////////////////// pot for L/R
int potLR=A0;    //A0 is pot for left or right 
int valLR = 0;   //value for input of pot 
int numberLR=0;  //Is it Lefr or right  
///////////////////////////////////////////  
/////////////////////////////////////////////////////////////////Servo/Mag
#include <Servo.h>
#include <Wire.h>
#define address 0x1E  //0011110b, I2C 7bit address of HMC5883
Servo myServo;  // create servo object to control a servo
long PreviousOnMilliS = 0; //will store last time servo was on
int   magX, magY, magZ;               // triple axis data
float theta;                          // magnetometer angle in PCB plane (around Z-axis) in degrees
float thetaOffset = -35;              // a value of 245 makes 0 degrees represent North
float servoAngle;                     // desired angle for servo
int kevin = 1;                        // to have a neg distance ONCE
////////////////////////////////////////////


void setup() {
  Serial.begin(9600);
  Wire.begin();
  Wire.beginTransmission(address);  //open communication with HMC5883
  Wire.write(0x02);                 //select mode register
  Wire.write(0x00);                 //continuous measurement mode
  Wire.endTransmission();
  ////////////////////////////////////////
  pinMode(piston, OUTPUT); //pin 13 is piston
  myServo.attach(3);  // attaches the servo on pin 3 to the servo object
  pinMode(reedSwitch, INPUT_PULLUP); //reedswitch pin2
  delay(3000);}                       ///////////////THERES'S A 3 SEC DELAY////////////////




void loop() {
  
  unsigned long CurrentMilliP = millis(); //The moment the arduino is on it starts counting. This one is for piston
  unsigned long CurrentMilliS = millis();  //This counting is for servo
  unsigned long CurrentMilliR = millis(); //for reed switch
  val = analogRead(pot);
  int mappedVal = map(val, 0, 1023, 0, 255);
  valLR = analogRead(potLR);
  int mappedValLR =map(valLR,0,1023,0,255);

  ///////////////////////////////////////////////////////////////
  if (CurrentMilliR - PreviousOnMilliR >reedinterval){
  readReedSwitch = digitalRead(reedSwitch); //data from reedSwitch is called ReadReedSwitch
  PreviousOnMilliR=CurrentMilliR;
  Wall = readReedSwitch ; //wall without magnet is always 1
  if (Wall == 0 && PastWall == 1) { // == is for comparison. && means 'and'
    readReedSwitch = 1;
    CurrentreadReedSwitch = readReedSwitch + oldreadReedSwitch; //calculating the current distnace it travels(0+1+2+3+etc)
    CurrentCurrentreadReedSwitch = CurrentreadReedSwitch * 2.45  ; //MATH MAY HAVE TO CHANGE DEPENDING ON PLACEMENT OF REED SWITCH(calculating how much it really travelled)
    distance=CurrentCurrentreadReedSwitch+test ;
    oldreadReedSwitch = CurrentreadReedSwitch;}
  PastWall = Wall;}
///////////////////////////////////////////////////////////////Piston
  
  if (CurrentMilliP - PreviousOnMilliP > interval) { //Has the time from previous ON pass the interval you set
    PreviousOnMilliP = CurrentMilliP; //if the piston is on, turn it off. Vice versa. PreviousOnMilli is changed to the new time it was turned on
      if (state == LOW) //the double equal means if the state is low
        state = HIGH;
      else
        state = LOW;
    digitalWrite(piston, state); } //actually turn on/off the pin/piston

///////////////////////////////////////////////////////////////Left
if (mappedValLR < 130){
Serial.print("Left or Right: Left     ");
  /////////////////////////////////////////////////////////////MODE 1
  if (CurrentMilliS - PreviousOnMilliS > servointerval && mappedVal < 90 &&  distance<60) {
    number = 1; PreviousOnMilliS = CurrentMilliS; updateMag();
    
  theta = 180. / 3.14159 * atan2(magY, magX) + thetaOffset; // convert to degrees, apply offset
  theta = modulo(theta, 360.); // ensure that theta is between 0 and 360 degrees
  servoAngle = (theta +60); 
  servoAngle = modulo(servoAngle, 360.);
  if (servoAngle > 270){
    servoAngle = 0; }
  else if (servoAngle > 180){
    servoAngle = 180;}
  myServo.write(servoAngle);}
  
//////////////////////////////////////////////////////////////MODE 2
  if (CurrentMilliS - PreviousOnMilliS > servointerval && mappedVal > 91 && mappedVal < 240 &&  distance<60){ 
    number = 2; PreviousOnMilliS = CurrentMilliS; updateMag();
    
    if(kevin == 1)
    {kevin=0;test= -24;}
    
  theta = 180. / 3.14159 * atan2(magY, magX) + thetaOffset; // convert to degrees, apply offset
  theta = modulo(theta, 360.); // ensure that theta is between 0 and 360 degrees
  servoAngle = (theta+ 60); 
  servoAngle = modulo(servoAngle, 360.);
  if (servoAngle > 270){
    servoAngle = 0; }
  else if (servoAngle > 180){
    servoAngle = 180;}
  myServo.write(servoAngle);}

//////////////////////////////////////////////////////////////MODE 3
  if (CurrentMilliS - PreviousOnMilliS > servointerval && mappedVal > 240  &&  distance<60) { 
    number = 3; PreviousOnMilliS = CurrentMilliS; updateMag();

    if(kevin == 1)
    {kevin=0;test= -39;}

  theta = 180. / 3.14159 * atan2(magY, magX) + thetaOffset; // convert to degrees, apply offset
  theta = modulo(theta, 360.); // ensure that theta is between 0 and 360 degrees
  servoAngle = (theta+60); 
  servoAngle = modulo(servoAngle, 360.);
  if (servoAngle > 270){
    servoAngle = 0; }
  else if (servoAngle > 180){
    servoAngle = 180;}
  myServo.write(servoAngle);}
  
/////////////////////////////////////////////////////////////MODE4 
if (CurrentMilliS - PreviousOnMilliS > servointerval && distance>61){
  number = 4; PreviousOnMilliS = CurrentMilliS; updateMag();
  
  theta = 180. / 3.14159 * atan2(magY, magX) + thetaOffset; // convert to degrees, apply offset
  theta = modulo(theta, 360.); // ensure that theta is between 0 and 360 degrees
  servoAngle = (theta +150); 
  servoAngle = modulo(servoAngle, 360.);
  if (servoAngle > 270){
    servoAngle = 0; }
  else if (servoAngle > 100){
    servoAngle = 100;}
  myServo.write(servoAngle);}}  



///////////////////////////////////////////////////////////////Right
if (mappedValLR > 132){
Serial.print("Left or Right: Right     ");
  /////////////////////////////////////////////////////////////MODE 1
  if (CurrentMilliS - PreviousOnMilliS > servointerval && mappedVal < 90 &&  distance<60) {
    number = 1; PreviousOnMilliS = CurrentMilliS; updateMag();
    
  theta = 180. / 3.14159 * atan2(magY, magX) + thetaOffset; // convert to degrees, apply offset
  theta = modulo(theta, 360.); // ensure that theta is between 0 and 360 degrees
  servoAngle = (theta -160); 
  servoAngle = modulo(servoAngle, 360.);
  if (servoAngle > 270){
    servoAngle = 0; }
  else if (servoAngle > 180){
    servoAngle = 180;}
  myServo.write(servoAngle);}
  
//////////////////////////////////////////////////////////////MODE 2
  if (CurrentMilliS - PreviousOnMilliS > servointerval && mappedVal > 91 && mappedVal < 240 &&  distance<60){ 
    number = 2; PreviousOnMilliS = CurrentMilliS; updateMag();
    
    if(kevin == 1)
    {kevin=0;test= -24;}
    
  theta = 180. / 3.14159 * atan2(magY, magX) + thetaOffset; // convert to degrees, apply offset
  theta = modulo(theta, 360.); // ensure that theta is between 0 and 360 degrees
  servoAngle = (theta-160); 
  servoAngle = modulo(servoAngle, 360.);
  if (servoAngle > 270){
    servoAngle = 0; }
  else if (servoAngle > 180){
    servoAngle = 180;}
  myServo.write(servoAngle);}

//////////////////////////////////////////////////////////////MODE 3
  if (CurrentMilliS - PreviousOnMilliS > servointerval && mappedVal > 240  &&  distance<60) { 
    number = 3; PreviousOnMilliS = CurrentMilliS; updateMag();

    if(kevin == 1)
    {kevin=0;test= -39;}

  theta = 180. / 3.14159 * atan2(magY, magX) + thetaOffset; // convert to degrees, apply offset
  theta = modulo(theta, 360.); // ensure that theta is between 0 and 360 degrees
  servoAngle = (theta-160); 
  servoAngle = modulo(servoAngle, 360.);
  if (servoAngle > 270){
    servoAngle = 0; }
  else if (servoAngle > 180){
    servoAngle = 180;}
  myServo.write(servoAngle);}
  
/////////////////////////////////////////////////////////////MODE4 
if (CurrentMilliS - PreviousOnMilliS > servointerval && distance>61){
  number = 4; PreviousOnMilliS = CurrentMilliS; updateMag();
  
  theta = 180. / 3.14159 * atan2(magY, magX) + thetaOffset; // convert to degrees, apply offset
  theta = modulo(theta, 360.); // ensure that theta is between 0 and 360 degrees
  servoAngle = (theta -220); 
  servoAngle = modulo(servoAngle, 360.);
  if (servoAngle > 200){
    servoAngle = 150; }
 
  myServo.write(servoAngle);}}  

    


///////////////////////////////////////////////////////////////////////////////////////////////  
  Serial.print("modeRN:");Serial.print(number);Serial.print("  ");
  Serial.print( distance);Serial.print(" inches   ");
  Serial.print( "Running Time:");Serial.print(CurrentMilliS);Serial.print("   ModePot:");Serial.print(mappedVal);
  Serial.print("      theta"); Serial.print("\t"); Serial.print(theta); Serial.print("\t");
  Serial.print("servo angle:"); Serial.print("\t"); Serial.println(servoAngle);}//(ln is for new line)




























/******************** update magnetometer function ************************/
void updateMag(void){
  /* Tell the HMC5883L where to begin reading data */
  Wire.beginTransmission(address);
  Wire.write(0x03); //select register 3, X MSB register
  Wire.endTransmission();
  /* Read data from each axis, 2 registers per axis */
  Wire.requestFrom(address, 6);
  if(6<=Wire.available()){
    magX = Wire.read()<<8;  //X msb
    magX |= Wire.read();    //X lsb
    magZ = Wire.read()<<8;  //Z msb
    magZ |= Wire.read();    //Z lsb
    magY = Wire.read()<<8;  //Y msb
    magY |= Wire.read();    //Y lsb
  }
}



/************************* modulo for floats *******************************/
float modulo(float dividend, float divisor) {
  float quotient = floor(dividend/divisor); // find quotient rounded down
  return dividend - divisor*quotient; // return the remainder of the divison
}