diff --git a/driver_2_0/driver_2_3/driver_2_3.ino b/driver_2_0/driver_2_3/driver_2_3.ino new file mode 100644 index 0000000..c317ea9 --- /dev/null +++ b/driver_2_0/driver_2_3/driver_2_3.ino @@ -0,0 +1,251 @@ +/* + +Timelapse Dolly - Automation, Systemer og Komponenter +@version 2.1 +@date 2014-05-05 +@author Peter Savnik + +@Description +Make a step motor control motion of a Camera mounted on a dolly. +Input: Distance and Time + + +*/ +#include +#include + +const int DEBUG = 0; // Toogle Serial output +const int PIN_RUN = 8; +const int PIN_STEP = 9; +const int PIN_ENABLE = A0; +const int PIN_TIME_SCALE_POT = A1; +const float Pi = 3.14159; + +const float STEP_RES = 16.0; // Stepper resolution +const float STEPS_PR_ROUND = 200.0*STEP_RES; // Total steps pr. round +const float RADIUS = 0.0272; // [m] +const float DIST_PR_STEP = (2.0*RADIUS*Pi)/STEPS_PR_ROUND; //[cm] Omkreds/steps pr round + + +float distance = 0.0; // [m] Record the number dist we've taken +float distance_to_go = 2.0; // [m] MAX:4.295*10^9 MIN:5.3407*10^-5 +unsigned long steps_to_go = (distance_to_go)/DIST_PR_STEP; // total amount of steps to the distance_to_go +float time_to_go = 60; // [s] time to do the distance +unsigned long steps = 0; // Total count of steps +int run = 0; // Control var for start/stop fcn +int dir = 1; // Control direction +int sensorValue = 0; // Time scale pot sensor +int stepState = 0; // Step output state + +String state = "IDLE", next_state = "IDLE"; // Statemachine control + +float time_pr_step; +float time_pr_step_ms; +float time_pr_step_micros; +long time_start = 0; +long time_end = 0; +int use_micros_time = 0; + +unsigned long current_time = 0; +unsigned long prev_time = 0; + +void setup() { + Serial.begin(115200); + /*while(!Serial){ + ; // wait for serial to begin + }*/ + Serial.println('Timelapse Dolly Start'); + + + pinMode(PIN_RUN, INPUT_PULLUP); + pinMode(PIN_STEP, OUTPUT); + pinMode(PIN_ENABLE, OUTPUT); + pinMode(PIN_TIME_SCALE_POT, INPUT); + digitalWrite(PIN_STEP, LOW); + digitalWrite(PIN_ENABLE, HIGH); + + //establishContact(); // send a byte to establish contact until receiver responds +} + +void loop() { + + run = digitalRead(PIN_RUN); + + // Statemachine! + if (state == "IDLE"){ + // Wait for action + + // Calc delay time + time_pr_step = time_to_go/steps_to_go; // the delay time between steps [s]/[step] + time_pr_step_ms = time_pr_step*pow(10,3); // Convert to millisec [ms]/[step] + time_pr_step_micros = time_pr_step*pow(10,6); // Convert to millisec [micros]/[step] + if (time_pr_step_ms < 1) time_pr_step_ms = 1; // Don't go too fast! saturation + if (time_pr_step_micros < 1) time_pr_step_micros = 1; // Don't go too fast! saturation + + if(time_to_go < 60*60) use_micros_time = 1; // 60[m]*60[s] use micros instead of millis + + // Print Parameters + /*Serial.print("Dist: "); + Serial.print(distance_to_go); + Serial.print(", Steps to go:"); + Serial.print(steps_to_go); + Serial.print(", Time: "); + Serial.print(time_to_go); + Serial.print(", Step Time: "); + Serial.println(time_pr_step_ms);*/ + + // Change state + // Start running + if(run == LOW){ + digitalWrite(PIN_ENABLE, LOW); + time_start = millis(); + next_state = "MOVE"; + } + else{ + next_state = "IDLE"; + } + + } + else if (state == "MOVE"){ + next_state = "MOVE"; + // MOVE + // Check to see if we are at the end of our move + if ((steps*DIST_PR_STEP < distance_to_go) && (run == LOW)) { + + + if(use_micros_time == 1){ + current_time = micros(); + if(current_time-prev_time >= time_pr_step_micros/2){ + prev_time = current_time; + // Step toogle + if(stepState == 0){ + digitalWrite(PIN_STEP, HIGH); // step high + stepState = 1; + } + else if(stepState == 1){ + digitalWrite(PIN_STEP, LOW); // step low + stepState = 0; + steps = 1+steps; // Incremetent steps + //Serial.println(steps_to_go-steps); + } // end step state + + } // if time + + } // if use micro + else{ + current_time = millis(); + if(current_time-prev_time >= time_pr_step_ms/2){ + prev_time = current_time; + // Step toogle + if(stepState == 0){ + digitalWrite(PIN_STEP, HIGH); // step high + stepState = 1; + } + else if(stepState == 1){ + digitalWrite(PIN_STEP, LOW); // step low + stepState = 0; + steps = 1+steps; // Incremetent steps + //Serial.println(steps_to_go-steps); + } // end step state + + } // if time + }// else use micros + + } // if dist < + + // If dist is done go to wait state + if (steps*DIST_PR_STEP >= distance_to_go){ + next_state = "DONE"; + digitalWrite(PIN_ENABLE, HIGH); // disable motor + // calc run time + time_end = millis(); + Serial.print("Time: "); + Serial.println((time_end-time_start)*pow(10,-3)); //[s] + steps = 0; // reset dist + + } // if dist >= + + + // Change state + if(run == HIGH){ // Stop running + next_state = "IDLE"; + digitalWrite(PIN_ENABLE, HIGH); // disable motor + } + + } // state + + else if(state == "DONE"){ + next_state = "DONE"; + // If off go to idle + if(run == HIGH) next_state = "IDLE"; + + } // state DONE + else { + Serial.print("Error in statemachine: "); + Serial.println(state); + } + if(state != next_state) Serial.println(next_state); + state = next_state; + + + + + + /* + // If dist is done disable + if ((steps*DIST_PR_STEP >= distance_to_go)){ + disableMotor(); + steps = 0; + } + */ +} + + +void enableMotor(){ + // Enable motor driver + digitalWrite(PIN_ENABLE, LOW); + if(DEBUG == 1) Serial.println("Start"); +} + +void disableMotor(){ + // Enable motor driver + digitalWrite(PIN_ENABLE, HIGH); + if(DEBUG == 1) Serial.println("Stop"); +} + + +void enableMotorToogle(){ + // Enable motor driver + if(run == 0){ + enableMotor(); + run = 1; + } + else if(run == 1){ + disableMotor(); + run = 0; + } +} + + + + +void establishContact() { + while (Serial.available() <= 0) { + Serial.println("0,0,0"); // send an initial string + delay(300); + } +} + +/* +|| @changelog +|| | 2.2 2014-05-08 - Peter Savnik : Time function fix +|| | 2.1 2014-05-08 - Peter Savnik : Dist is calibrated +|| | 2.0 2014-05-06 - Peter Savnik : Added state machine & Millis as delay +|| | 1.3 2014-05-05 - Peter Savnik : Added Potentiometer control of time +|| | 1.2 2014-05-01 - Peter Savnik : Added Debug function, enable serial output when debugging +|| | 1.1 2014-05-01 - Peter Savnik : Added direction control +|| | 1.0 2014-05-01 - Peter Savnik : Initial Release +|| # +*/ + +