/*
 Do several things at once

 This sketch will slowly move a servo to match a potentiometer and 
 simultaneously blink an LED 

 this will ultimately become an integrated lab for USAFA Astro 331

  Jordan Firth
  updated 2022-09-20
 */

#define LED_1_PIN 13


#include <Servo.h>          //include the servo library


#include <protothreads.h>   // arduino pseudo-multitasking 

// need a ptx for each "thread"
static struct pt pt1, pt2, pt3;


bool move = false; // flag for whether to move the servo
int potPosition[1] = {0}; // this variable will store the position of the potentiometer
long servoPosition[1];    // the servo will move to this position
int sign_of_change ;  // move the servo in the correct direction

Servo myservo;              //create a servo object
long motion; 
long rate = 10; // speed of servo motion
long delta; 

long blink_duration = 1000;


// begin defining "threads"


// PT1 to blink LED
static int protothreadBlinkLED1(struct pt *pt){
  static long nextTimeBlink = blink_duration;
  
  PT_BEGIN(pt);

    // PT_WAIT_UNTIL is the protothread equivalent of delay(x)
    // wait for xx time then turn on LED
    PT_WAIT_UNTIL(pt, millis() > nextTimeBlink ); 
    nextTimeBlink+= blink_duration; //update next blink time
    digitalWrite(LED_1_PIN, HIGH);

    // wait for xx time then turn off LED
    PT_WAIT_UNTIL(pt, millis() >nextTimeBlink);
    nextTimeBlink+= blink_duration; //update next blink time
    digitalWrite(LED_1_PIN, LOW);
  PT_END(pt)
} //end protothreadBlinkLED1 (PT1)



/* protothread shell
// PT1 to do XX
static int protothreadName(struct pt *pt){
  static long nextTimeBlink = blink_duration;
  
  PT_BEGIN(pt);

    // PT_WAIT_UNTIL is the protothread equivalent of delay(x)
    // wait for xx time then turn on LED
    PT_WAIT_UNTIL(pt, millis() > nextTimeBlink ); 
    nextTimeBlink+= blink_duration; //update next blink time
    digitalWrite(LED_1_PIN, HIGH);

    // wait for xx time then turn off LED
    PT_WAIT_UNTIL(pt, millis() >nextTimeBlink);
    nextTimeBlink+= blink_duration; //update next blink time
    digitalWrite(LED_1_PIN, LOW);
  PT_END(pt)
} //end protothreadBlinkLED1 (PT1)
*/

long step_duration = 500;
// PT2 to move servo
static int protothread_move_servo(struct pt *pt) {
  static unsigned long next_time_move = step_duration; 
  PT_BEGIN(pt);
    //last_time_move = millis();
    PT_WAIT_UNTIL(pt, millis() > next_time_move);
  if(move){
servoPosition[1] = map(potPosition[0], 0, 1023, 20, 160); //convert the potentiometer number to a servo position from 20-160
 //Note: its best to avoid driving the little SIK servos all the
 //way to 0 or 180 degrees it can cause the motor to jitter, which is bad for the servo.

// only move if commanded position exceeds deadband
  if(abs(servoPosition[1] - servoPosition[0]) >10 ){
    //sign_of_change = (servoPosition[1] > 0) - (servoPosition[1] < 0);

    delta = servoPosition[1]-servoPosition[0]; 
    sign_of_change = delta/abs(delta);
    motion = rate * sign_of_change;
    servoPosition[0]= servoPosition[0] + motion;

  } // end if
  myservo.write(servoPosition[0]);  // move the servo to the 10 degree position
  
  next_time_move+= step_duration
  PT_END(pt);

  } // end if(move)

} // end protothread_move_servo (PT2)



// PT3 to print commanded position via serial
  static int protothread_print_pot(struct pt *pt){
  static unsigned long last_time_print = 0;
  PT_BEGIN(pt);
  while(1) {
    last_time_print = millis();
    PT_WAIT_UNTIL(pt, millis() - last_time_print > 500);
    
 Serial.print(String(servoPosition[0], 5));     Serial.print("\n");
    
  }
  PT_END(pt);
}
  
void setup() {

    Serial.begin(115200);

  Serial.println("Initialize");



  myservo.attach(9);        //tell the servo object that its servo is plugged into pin 9

  pinMode(LED_1_PIN, OUTPUT);
  PT_INIT(&pt1);

}

void loop() {

  protothreadBlinkLED1(&pt1);

  read_pot();
  protothread_move_servo(&pt2);
  protothread_print_pot(&pt3);

  }

// get pot position
void read_pot() {
  potPosition[1] = analogRead(A0);                     //use analog read to measure the position of the potentiometer (0-1023)
  if(abs(potPosition[1]-potPosition[0])>10){
    potPosition[0] = potPosition[1];
    move=true;
  } // end if
} // end read_pot