// Written By: Christina Morrow
// Date: 2/10/2021
// Email: cjm18bn@my.fsu.edu

// FAMU-FSU College of Engineering
// Senior Design Team 517

// This program is the automated demonstration of the ARROW 
// (Automated and Ranged Relocation Of the LSMS for Wider application)

////////////////////////////////////////////////////////////////////////
//////////////////////// DEMO INSTRUCTIONS /////////////////////////////
////////////////////////////////////////////////////////////////////////

// Steps to run this demonstration:
//    1. Go to https://www.arduino.cc/en/software and download the latest version of the arduino IDE.
//    2. Open this code using the arduino IDE.
//    3. Plug the power cord into the lower port in the ARROW and connect to wall power. 
//    4. Plug the arduino USB to the upper port in the ARROW and connect to you computer. 
//    5. Upload the code (the horizontal arrow in the upper left of the screen).
//    6. Open the Serial Monitor (the magnifying glass in the upper right of the screen).
//    7. Follow the prompts in the Serial Monitor. 

// NOTE ON RESET: 
//    1. If the connector plate is NOT horizontal, set duty = -255 and reupload the code. 
//    2. If the connector plate is IS horizontal, set duty = 255 and reupload the code.
//    3. The program will loop, allowing you to reset the ARROW. The ARROW will reset
//       every other iteration of the code.   

int duty = 255;   // The duty cycle describes the amount of time the signal 
                  // is in a high (ON) state as a percentage of the total 
                  // time of it takes to complete one cycle. 
                
////////////////////////////////////////////////////////////////////////
//////////////////////INITIALZIE VARIABLES//////////////////////////////
////////////////////////////////////////////////////////////////////////

// initialize RGB LED1
const int ledR_LSMS = 9;    // assign pin 9 to RED channel 
const int ledG_LSMS = 8;    // assign pin 8 to GREEN channel 
const int ledB_LSMS = 7;    // assign pin 7 to BLUE channel 
int R_LSMS = 0;             // variable that controls RED channel 
int G_LSMS = 0;             // variable that controls GREEN channel 
int B_LSMS = 0;             // variable that controls BLUE channel 
          
int state = 0;              // variable that controls the switch
char inChar;                // allows user to hit the space bar to continue
int reset = 0;              // variable used to reset the position of the ARROW
int iter = 0;               // iteration variable
unsigned int ticks = 0;     // variable to keep track of tim

void pwm_init(void);        // initialize pulse width modulation
void my_motor0(int duty);   // function to 
void set_pwm0(int duty);    // set pulse width modulation for first linear actuator
void set_pwm1(int duty);    // set pulse width modulation for second linear actuator
int dutySigned = 0;         // variable to change the direction of the linear actuators

// include libraries
#include <stdio.h>
#include <stdlib.h>

////////////////////////////////////////////////////////////////////////
///////////////////////SETUP (RUNS OUNCE)///////////////////////////////
////////////////////////////////////////////////////////////////////////

void setup() {

  DDRC = 0x00;            // all pins of PORTC are inputs
  PORTC = 0XFF;           // enable pull up resistors
  DDRB = 0xFF;            // all pins of PORTC are outputs for motor direction
  
  pwm_init();             // initialize pulse width modulation for DC motor control

  pinMode(ledR_LSMS, OUTPUT);  // set pin mode to output for RED channel
  pinMode(ledG_LSMS, OUTPUT);  // set pin mode to output for GREEN channel
  pinMode(ledB_LSMS, OUTPUT);  // set pin mode to output for BLUE channel 

  Serial.begin(9600);           // set data rate to 9600 bits per second
  
}


////////////////////////////////////////////////////////////////////////
//////////////////////MAIN (RUNS CONTINOUSLY)///////////////////////////
////////////////////////////////////////////////////////////////////////

void loop() {

  switch(state)
  {
    case 0: // off and idle
    
      Serial.println(" ");
      Serial.println("STATE 0: ARROW is Idle"); 

      // turn off light
      R_LSMS = 0;                       // turn RED channel OFF
      G_LSMS = 0;                       // turn GREEN channel OFF
      B_LSMS = 0;                       // turn BLUE channel OFF
      analogWrite(ledR_LSMS,R_LSMS);    // push LED output for RED channel
      analogWrite(ledG_LSMS,G_LSMS);    // push LED output for GREEN channel
      analogWrite(ledB_LSMS,B_LSMS);    // push LED output for BLUE channel   

      // this loop pauses the code until the user is ready to continue
      Serial.println("Please Press [SPACE] and then [ENTER] to continue.");

      while(true)                   // remain here until told to break
      {
        if(Serial.available() > 0)  // is there user input?
          if(Serial.read() == 32)   // did they press the space bar?
            break;
      }

      reset++;                      // increase reset by one
      state = 1;                    // send to state 1
      break;
      
    case 1: // LSMS Powered and Connected to ARROW
    
      Serial.println(" ");
      Serial.println("STATE 1: LSMS Powered and Connected to ARROW");

      // LSMS is connected to power
      R_LSMS = 255;                     // turn RED channel ON
      G_LSMS = 255;                     // turn GREEN channel ON
      B_LSMS = 255;                     // turn BLUE channel ON
      analogWrite(ledR_LSMS,R_LSMS);    // push LED output for RED channel
      analogWrite(ledG_LSMS,G_LSMS);    // push LED output for GREEN channel
      analogWrite(ledB_LSMS,B_LSMS);    // push LED output for BLUE channel 
      
      // this loop pauses the code until the user is ready to continue
      Serial.println("Please Press [SPACE] and then [ENTER] to continue.");

      while(true)                   // remain here until told to break
      {
        if(Serial.available() > 0)  // is there user input?
          if(Serial.read() == 32)   // did they press the space bar?
            break;
      }   

      Serial.println("");
      Serial.println("The LSMS is sending a ready signal.");
      for(int i=0;i<5;i++)               // loop to create blinking LED "sending ready signal"
      {
        R_LSMS = 0;                       // turn RED channel OFF
        G_LSMS = 0;                       // turn GREEN channel OFF
        B_LSMS = 0;                       // turn BLUE channel ON
        analogWrite(ledR_LSMS,R_LSMS);    // push LED output for RED channel
        analogWrite(ledG_LSMS,G_LSMS);    // push LED output for GREEN channel
        analogWrite(ledB_LSMS,B_LSMS);    // push LED output for BLUE channel  
        delay(500);                       // delay for half a second
        
        R_LSMS = 255;                     // turn RED channel ON
        G_LSMS = 255;                     // turn GREEN channel ON
        B_LSMS = 255;                     // turn BLUE channel ON
        analogWrite(ledR_LSMS,R_LSMS);    // push LED output for RED channel
        analogWrite(ledG_LSMS,G_LSMS);    // push LED output for GREEN channel
        analogWrite(ledB_LSMS,B_LSMS);    // push LED output for BLUE channel  
        delay(500);                       // delay for half a second
      }
      
      state = 2;                          // send to state 2
      break;
      
    case 2: // SPIN the LSMS
    
      Serial.println(" ");
      Serial.println("STATE 2: SPIN the LSMS");

      // this loop pauses the code until the user is ready to continue
      Serial.println("Please Press [SPACE] and then [ENTER] to continue.");

      while(true)                   // remain here until told to break
      {
        if(Serial.available() > 0)  // is there user input?
          if(Serial.read() == 32)   // did they press the space bar?
            break;
      }             

      // SPIN the LSMS
      iter = 0;                           // reinitialize iteration variable to zero
      dutySigned = (-1^(reset+1))*duty;   // change the sign of duty based on the reset (reverse direction)
      while(iter < 1800)
      {
          my_motor0(dutySigned);          // activate linear (spinning) actuator
          iter++;                         // increase the iteration 
      }
      my_motor0(0);                       // stop the linear (spinning) actuator
      break; 

    case 3: // LOWER the LSMS
    
      Serial.println(" ");
      Serial.println("STATE 3: LOWER the LSMS");
      
      // this loop pauses the code until the user is ready to continue
      Serial.println("Please Press [SPACE] and then [ENTER] to continue.");

      while(true)                   // remain here until told to break
      {
        if(Serial.available() > 0)  // is there user input?
          if(Serial.read() == 32)   // did they press the space bar?
            break;
      }  

      // lower the LSMS
      iter = 0;                     // reinitialize iteration variable to zero
      dutySigned = (-1^reset)*duty; // change the sign of duty based on the reset (reverse direction)
      while(iter < 3050)
      {
        my_motor1(dutySigned);      // activate linear (lowering) actuator
        iter++;                     // increase the iteration 
      }
      my_motor0(0);                 // stop the linear (spinning) actuator
      break; 
    }
}


void pwm_init(void) {
  DDRL = 255; //PWM pins connected to port L
  TCCR5A = _BV(COM5A1) | _BV(COM5B1) | _BV(WGM52) | _BV(WGM50);
  TCCR5B = _BV(CS51) | _BV(CS50); //set prescaler to 64
  OCR5A = 0;
  OCR5B = 0;
}

//function that accepts positive and negative values for duty
void my_motor0(int duty)
{
  if (duty >= 0)
  {
    PORTB = 0b00000001;
    set_pwm0(duty);

  }
  if (duty < 0)
  {
    duty = duty * -1;
    PORTB = 0b00000010;
    set_pwm0(duty);

  }
}

//function that accepts positive and negative values for duty
void my_motor1(int duty)
{
  if (duty >= 0)
  {
    PORTB = 0b00001000;
    set_pwm1(duty);
  }
  if (duty < 0)
  {
    duty = duty * -1;
    PORTB = 0b00000100;
    set_pwm1(duty);
  }
}

//pin 46 of the Mega Board
void set_pwm0(int duty)
{
  OCR5A = duty;
}

//pin 45 of the Mega Board
void set_pwm1(int duty)
{
  OCR5B = duty;
}

// timer function
void setup_timer(void)
{
  noInterrupts();
  TCCR1A = 0; 
  TCCR1B = 0; 
  TCNT1  = 0;  
  OCR1A  = 625;   //compare match register 16MHz/256 
  TCCR1B |= (1 << WGM12);   // CTC mode 
  TCCR1B |= (1 << CS12);    // 256 prescaler
  TIMSK1 |= (1 << OCIE1A);  // enable timer compare interrupt 
  interrupts();
}

ISR(TIMER1_COMPA_vect) // timer compare interrupt service routine
{
  ticks++; // increase ticks by 1
}