diff --git a/pan_tilt_mount_nano_code/panTiltMount.cpp b/pan_tilt_mount_nano_code/panTiltMount.cpp new file mode 100644 index 0000000..cbaa930 --- /dev/null +++ b/pan_tilt_mount_nano_code/panTiltMount.cpp @@ -0,0 +1,760 @@ +#include "PanTiltMount.h" +#include //A library I created for Arduino that contains some simple functions I commonly use. Library available at: https://github.com/isaac879/Iibrary +#include +#include +#include //To be able to save values when powered off + +/*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +//Global scope +AccelStepper stepper_pan = AccelStepper(1, PIN_STEP_PAN, PIN_DIRECTION_PAN); +AccelStepper stepper_tilt = AccelStepper(1, PIN_STEP_TILT, PIN_DIRECTION_TILT); + +MultiStepper multi_stepper; + +ArrayElement program_elements[ARRAY_LENGTH]; + +int moves_array_elements = 0; +int current_moves_array_index = -1; +char stringText[MAX_STRING_LENGTH + 1]; +float pan_steps_per_degree = (200.0 * SIXTEENTH_STEP * PAN_GEAR_RATIO) / 360.0; //Stepper motor has 200 steps per 360 degrees +float tilt_steps_per_degree = (200.0 * SIXTEENTH_STEP * TILT_GEAR_RATIO) / 360.0; //Stepper motor has 200 steps per 360 degrees +int step_mode = 1; +bool enable_state = true; +long limit_pan_min = -2147483648; +long limit_pan_max = 2147483647; +long limit_tilt_min = -2147483648; +long limit_tilt_max = 2147483647; +float pan_acceleration = 5000; +float tilt_acceleration = 5000; +float hall_pan_offset_degrees = 0; +float hall_tilt_offset_degrees = 0; +byte invert_pan = 0; +byte invert_tilt = 0; +byte enable_homing = 0; +float pan_max_speed = 2000; +float tilt_max_speed = 2000; +long target_position[2]; + +/*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +void initPanTilt(void){ + Serial.begin(BAUD_RATE); + pinMode(PIN_MS, OUTPUT); + pinMode(PIN_ENABLE, OUTPUT); + pinMode(PIN_DIRECTION_PAN, OUTPUT); + pinMode(PIN_STEP_PAN, OUTPUT); + pinMode(PIN_DIRECTION_TILT, OUTPUT); + pinMode(PIN_STEP_TILT, OUTPUT); + pinMode(PIN_DIRECTION_SLIDER, OUTPUT); + pinMode(PIN_STEP_SLIDER, OUTPUT); + pinMode(PIN_PAN_HALL, INPUT_PULLUP); + pinMode(PIN_TILT_HALL, INPUT_PULLUP); + setEEPROMVariables(); + stepper_pan.setMinPulseWidth(20); + stepper_tilt.setMinPulseWidth(20); + setStepMode(step_mode); //steping mode + stepper_pan.setMaxSpeed(pan_max_speed); + stepper_pan.setAcceleration(pan_acceleration); + stepper_tilt.setMaxSpeed(tilt_max_speed); + stepper_tilt.setAcceleration(tilt_acceleration); + invertPanDirection(invert_pan); + invertTiltDirection(invert_tilt); + multi_stepper.addStepper(stepper_pan); + multi_stepper.addStepper(stepper_tilt); + enableSteppers(true); + printi(F("Setup complete.\n")); + if(enable_homing == 1){ + printi(F("Beginning homing...\n")); + if(findHome()){ + printi(F("Homing complete.\n")); + } + else{ + stepper_pan.setCurrentPosition(0); + stepper_tilt.setCurrentPosition(0); + printi(F("Error finding home position... Current position has been set as home.\n")); + } + } +} + +/*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +float boundFloat(float value, float lower, float upper){ + if(value < lower){ + value = lower; + } + else if(value > upper){ + value = upper; + } + return value; +} + +/*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +void serialFlush(void){ + while(Serial.available() > 0){ + char c = Serial.read(); + } +} + +/*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +void enableSteppers(bool state){ + if(state == true){ + digitalWrite(PIN_ENABLE, LOW); //Enable the stepper drivers + printi(F("Stepper motors enabled.\n")); + } + else{ + digitalWrite(PIN_ENABLE, HIGH); //Disabe the stepper drivers + printi(F("Stepper motors disabled.\n")); + } + enable_state = state; +} + +/*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +void setStepMode(int mode){ + + if(mode == 0){//Full step mode + if(step_mode == 1){//was in sixteenth step mode + stepper_pan.setCurrentPosition(stepper_pan.currentPosition() / SIXTEENTH_STEP); + stepper_tilt.setCurrentPosition(stepper_tilt.currentPosition() / SIXTEENTH_STEP); + } + digitalWrite(PIN_MS, LOW); + pan_steps_per_degree = (200.0 * FULL_STEP * PAN_GEAR_RATIO) / 360.0; //Stepper motor has 200 steps per 360 degrees + tilt_steps_per_degree = (200.0 * FULL_STEP * TILT_GEAR_RATIO) / 360.0; //Stepper motor has 200 steps per 360 degrees + step_mode = 0; + printi(F("Set to Full Step mode.\n")); + } + else if(mode == 1){//Sixteenth step mode + if(step_mode == 0){//was in full step mode + stepper_pan.setCurrentPosition(stepper_pan.currentPosition() * SIXTEENTH_STEP); + stepper_tilt.setCurrentPosition(stepper_tilt.currentPosition() * SIXTEENTH_STEP); + } + digitalWrite(PIN_MS, HIGH); + pan_steps_per_degree = (200.0 * SIXTEENTH_STEP * PAN_GEAR_RATIO) / 360.0; //Stepper motor has 200 steps per 360 degrees + tilt_steps_per_degree = (200.0 * SIXTEENTH_STEP * TILT_GEAR_RATIO) / 360.0; //Stepper motor has 200 steps per 360 degrees + step_mode = 1; + printi(F("Set to Sixteenth Step mode.\n")); + } + else{ + printi(F("Invalid step mode... Enter a 0 for full step or 1 for sixteenth step mode.\n")); + } +} + +/*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +void panJogDegrees(float jogAngle){ + target_position[0] = panDegreesToSteps(jogAngle); + multi_stepper.moveTo(target_position); +} +/*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +void tiltJogDegrees(float jogAngle){ + target_position[1] = tiltDegreesToSteps(jogAngle); + multi_stepper.moveTo(target_position); +} + +/*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +float panDegreesToSteps(float angle){ + return pan_steps_per_degree * angle; +} + +/*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +float tiltDegreesToSteps(float angle){ + return tilt_steps_per_degree * angle; +} + +/*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +void statusReport(void){ + printi(F("----------Status Report----------\n")); + printi(F("Step Mode: "), step_mode); + printi(F("pan_steps_per_degree: "), pan_steps_per_degree, 3, F("\n")); + printi(F("tilt_steps_per_degree: "), tilt_steps_per_degree, 3, F("\n")); + printi(F("Battery percentage: "), getBatteryPercentage(), 3, F("%\n")); + +} + +/*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +void printProgramElements(void){ + printi(F("-----Program Elements-----\n")); + for(int row = 0; row < moves_array_elements; row++){ + printi(F(""), row, F(" |")); + printi(F(" Pan: "), program_elements[row].panStepCount, F(" steps\t")); + printi(F("Tilt: "), program_elements[row].tiltStepCount, F(" steps\t")); + printi(F("Pan Speed: "), program_elements[row].panSpeed, 3, F(" steps/sec\t")); + printi(F("Tilt Speed: "), program_elements[row].tiltSpeed, 3, F(" steps/sec\t")); + printi(F("Delay: "), program_elements[row].msDelay, F(" |\n")); + } + printi(F("\n")); +} + +/*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +void debugReport(void){ + printi(F("----------Debug Report----------\n")); + printi(F("Stepper enable state: "), enable_state); + printi(F("Step Mode: "), step_mode); + printi(F("Pan Hall sensor state: "), digitalRead(PIN_PAN_HALL)); + printi(F("Tilt Hall sensor state: "), digitalRead(PIN_TILT_HALL)); + printi(F("Pan step count: "), stepper_pan.currentPosition()); + printi("Tilt step count: ", stepper_tilt.currentPosition()); + printi(F("Pan angle: "), panStepsToDegrees(stepper_pan.currentPosition()), 3, F("degrees\n")); + printi(F("Tilt angle: "), tiltStepsToDegrees(stepper_tilt.currentPosition()), 3, F("degrees\n")); + printi(F("Pan steps per degree: "), pan_steps_per_degree); + printi(F("Tilt steps per degree: "), tilt_steps_per_degree); + printi(F("Pan current steps/second: "), stepper_pan.speed()); + printi(F("Tilt current steps/second: "), stepper_tilt.speed()); + printi(F("Pan maximum steps/second: "), stepper_pan.maxSpeed()); + printi(F("Tilt maximum steps/second: "), stepper_tilt.maxSpeed()); + printi(F("Pan acceleration: "), pan_acceleration); + printi(F("Tilt acceleration: "), tilt_acceleration); + printi(F("Pan invert direction: "), invert_pan); + printi(F("Tilt invert direction: "), invert_tilt); + printi(F("Pan Hall offset: "), hall_pan_offset_degrees, 3, F("degrees\n")); + printi(F("Tilt Hall offset: "), hall_tilt_offset_degrees, 3, F("degrees\n")); + printi(F("Battery voltage: "), getBatteryVoltage(), 3, F("V\n")); + printi(F("Battery percentage: "), getBatteryPercentage(), 3, F("%\n")); + printi(F("Homing on start-up: "), enable_homing); + printi(F("Version: ")); + printi(F(VERSION_NUMBER)); + printi(F("\n")); + printEEPROM(); + printProgramElements(); + printi(F("------------------------------\n")); +} + +/*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +int setTargetPositions(float panDeg, float tiltDeg){//TODO: limits + target_position[0] = panDegreesToSteps(panDeg); + target_position[1] = tiltDegreesToSteps(tiltDeg); + multi_stepper.moveTo(target_position); +} + +/*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +bool findHome(void){ + bool panHomeFlag = false; + bool tiltHomeFlag = false; + int panHomingDir = -1; + int tiltHomingDir = -1; + + while(digitalRead(PIN_PAN_HALL) == 0 || digitalRead(PIN_TILT_HALL) == 0){//If already on a Hall sensor move off + target_position[0] = target_position[0] + panDegreesToSteps(!digitalRead(PIN_PAN_HALL));//increment by 1 degree + target_position[1] = target_position[1] + tiltDegreesToSteps(!digitalRead(PIN_TILT_HALL));//increment by 1 degree + if(target_position[0] > panDegreesToSteps(360) && target_position[1] > tiltDegreesToSteps(360)){//If both axis have done more than a full rotation there must be an issue... + return false; + } + multi_stepper.moveTo(target_position); + multi_stepper.runSpeedToPosition(); + } + stepper_pan.setCurrentPosition(0);//set step count to 0 + stepper_tilt.setCurrentPosition(0);//set step count to 0 + + setTargetPositions(-45, -45); + while(multi_stepper.run()){ + if(digitalRead(PIN_PAN_HALL) == 0){ + stepper_pan.setCurrentPosition(0);//set step count to 0 + setTargetPositions(0, -45 * !tiltHomeFlag); + panHomeFlag = true; + panHomingDir = 1; + } + if(digitalRead(PIN_TILT_HALL) == 0){ + stepper_tilt.setCurrentPosition(0); + setTargetPositions(-45 * !panHomeFlag, 0); + tiltHomeFlag = true; + tiltHomingDir = 1; + } + } + + setTargetPositions(45 * !panHomeFlag, 45 * !tiltHomeFlag);//set angle to 0 for an axis if it's home + while(multi_stepper.run()){ + if(digitalRead(PIN_PAN_HALL) == 0){ + stepper_pan.setCurrentPosition(0);//set step count to 0 + setTargetPositions(0, 45); + panHomeFlag = true; + } + if(digitalRead(PIN_TILT_HALL) == 0){ + stepper_tilt.setCurrentPosition(0); + setTargetPositions(45 * !panHomeFlag, 0); + tiltHomeFlag = true; + } + } + + setTargetPositions(360 * !panHomeFlag, 360 * !tiltHomeFlag);//full rotation on both axis so it must pass the home position + while(multi_stepper.run()){ + if(digitalRead(PIN_PAN_HALL) == 0){ + stepper_pan.setCurrentPosition(0);//set step count to 0 + setTargetPositions(0, 360 * !tiltHomeFlag); + panHomeFlag = true; + } + if(digitalRead(PIN_TILT_HALL) == 0){ + stepper_tilt.setCurrentPosition(0); + setTargetPositions(360 * !panHomeFlag, 0); + tiltHomeFlag = true; + } + } + if(panHomeFlag && tiltHomeFlag){ + setTargetPositions(hall_pan_offset_degrees * panHomingDir, hall_tilt_offset_degrees * tiltHomingDir); + multi_stepper.runSpeedToPosition(); + stepper_pan.setCurrentPosition(0);//set step count to 0 + stepper_tilt.setCurrentPosition(0);//set step count to 0 + setTargetPositions(0, 0); + return true; + } + else{ + return false; + } +} + +/*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +float getBatteryVoltage(void){ //TODO: Calibrate the values for your battery + return mapNumber(analogRead(PIN_INPUT_VOLTAGE), 0, 1007, 0, 12.6);//1007 = 12.6V +} + +/*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +float getBatteryPercentage(void){ //TODO: Calibrate the values for your battery + return boundFloat(mapNumber(getBatteryVoltage(), 9, 12.6, 0, 100), 0, 100); //780 = 9V = 0%, 1023 = 12.6V = 100% +} + +/*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +//float panDegreesToSteps(float angularVelocity){ +// return angularVelocity * pan_steps_per_degree; +//} +// +///*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ +// +//float tiltDegreesToSteps(float angularVelocity){ +// return angularVelocity * tilt_steps_per_degree; +//} +// +///*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +float panStepsToDegrees(long steps){ + return steps / pan_steps_per_degree; +} + +/*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +float tiltStepsToDegrees(long steps){ + return steps / tilt_steps_per_degree; +} + +/*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +int addPosition(void){ + if(moves_array_elements >= 0 && moves_array_elements < ARRAY_LENGTH){ + program_elements[moves_array_elements].panStepCount = stepper_pan.currentPosition(); + program_elements[moves_array_elements].tiltStepCount = stepper_tilt.currentPosition(); + program_elements[moves_array_elements].panSpeed = stepper_pan.maxSpeed(); + program_elements[moves_array_elements].tiltSpeed = stepper_tilt.maxSpeed(); + current_moves_array_index = moves_array_elements; + moves_array_elements++;//increment the index + printi(F("Position added at index: "), current_moves_array_index); + return 0; + } + return -1; +} + +/*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +void clearArray(void){ + for(int row = 0; row < ARRAY_LENGTH; row++){ + program_elements[row].panStepCount = 0; + program_elements[row].tiltStepCount = 0; + program_elements[row].panSpeed = 0; + program_elements[row].tiltSpeed = 0; + program_elements[row].msDelay = 0; + } + moves_array_elements = 0; + current_moves_array_index = -1; + printi(F("All positions cleared.")); +} + +/*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +void moveToIndex(int index){ + if(index < moves_array_elements && index >= 0){ + target_position[0] = program_elements[index].panStepCount; + target_position[1] = program_elements[index].tiltStepCount; + stepper_pan.setMaxSpeed(program_elements[index].panSpeed); + stepper_tilt.setMaxSpeed(program_elements[index].tiltSpeed); + multi_stepper.moveTo(target_position); //Sets new target positions + multi_stepper.runSpeedToPosition(); //Moves and blocks until complete + delay(program_elements[index].msDelay); + current_moves_array_index = index; +// printi(F("Moved to index: "), current_moves_array_index); + } +} + +/*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +void executeMoves(int repeat){ + for(int i = 0; i < repeat; i++){ + for(int row = 0; row < moves_array_elements; row++){ + moveToIndex(row); + } + if(getBatteryVoltage() < 9.5){//9.5V is used as the cut off to allow for inaccuracies and be on the safe side. + delay(200); + if(getBatteryVoltage() < 9.5){//Check voltage is still low and the first wasn't a miscellaneous reading + printi(F("Battery low! Turn the power off.")); + while(1){}//loop and do nothing + } + } + } +} + +/*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +void gotoMovesArrayStart(void){ + moveToIndex(0); +} + +/*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +void gotoMovesArrayEnd(void){ + moveToIndex(moves_array_elements - 1); +} + +/*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +void editMovesArrayIndex(void){ + program_elements[current_moves_array_index].panStepCount = stepper_pan.currentPosition(); + program_elements[current_moves_array_index].tiltStepCount = stepper_tilt.currentPosition(); + program_elements[current_moves_array_index].panSpeed = stepper_pan.maxSpeed(); + program_elements[current_moves_array_index].tiltSpeed = stepper_tilt.maxSpeed(); + printi(F("Position edited at index: "), current_moves_array_index); +} + +/*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +void addDelay(unsigned int ms){ + program_elements[current_moves_array_index].msDelay = ms; + printi(ms, F("")); + printi(F("ms delay added at index: "), current_moves_array_index); +} + +/*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +void invertPanDirection(bool invert){ + printi(F("Setting pan inversion to: "), invert); + invert_pan = invert; + stepper_pan.setPinsInverted(invert, false, false); +} + +/*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +void invertTiltDirection(bool invert){ + printi(F("Setting tilt inversion to: "), invert); + invert_tilt = invert; + stepper_pan.setPinsInverted(invert, false, false); +} + +/*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +void saveEEPROM(void){ + EEPROM.put(EEPROM_ADDRESS_ENABLE_HOMING, enable_homing); + EEPROM.put(EEPROM_ADDRESS_MODE, step_mode); + EEPROM.put(EEPROM_ADDRESS_LIMIT_PAN_MIN, limit_pan_min); + EEPROM.put(EEPROM_ADDRESS_LIMIT_PAN_MAX, limit_pan_max); + EEPROM.put(EEPROM_ADDRESS_LIMIT_TILT_MIN, limit_tilt_min); + EEPROM.put(EEPROM_ADDRESS_LIMIT_TILT_MAX, limit_tilt_max); + EEPROM.put(EEPROM_ADDRESS_PAN_MAX_SPEED, pan_max_speed); + EEPROM.put(EEPROM_ADDRESS_TILT_MAX_SPEED, tilt_max_speed); + EEPROM.put(EEPROM_ADDRESS_PAN_ACCELERATION, pan_acceleration); + EEPROM.put(EEPROM_ADDRESS_TILT_ACCELERATION, tilt_acceleration); + EEPROM.put(EEPROM_ADDRESS_HALL_PAN_OFFSET, hall_pan_offset_degrees); + EEPROM.put(EEPROM_ADDRESS_HALL_TILT_OFFSET, hall_tilt_offset_degrees); + EEPROM.put(EEPROM_ADDRESS_INVERT_PAN, invert_pan); + EEPROM.put(EEPROM_ADDRESS_INVERT_TILT, invert_tilt); +} + +/*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +void printEEPROM(void){ + int itemp; + float ftemp; + long ltemp; + printi(F("-----Saved Values-----\n")); + EEPROM.get(EEPROM_ADDRESS_MODE, itemp); + if(itemp == 0){ + printi(F("Step mode: Full step\n")); + } + else if(itemp == 1){ + printi(F("Step mode: Sixteenth step\n")); + } + EEPROM.get(EEPROM_ADDRESS_LIMIT_PAN_MIN, ltemp); + printi(F("Pan min limit: "), ltemp); + EEPROM.get(EEPROM_ADDRESS_LIMIT_PAN_MAX, ltemp); + printi(F("Pan max limit: "), ltemp); + EEPROM.get(EEPROM_ADDRESS_LIMIT_TILT_MIN, ltemp); + printi(F("Tilt min limit: "), ltemp); + EEPROM.get(EEPROM_ADDRESS_LIMIT_TILT_MAX, ltemp); + printi(F("Tilt max limit: "), ltemp); + EEPROM.get(EEPROM_ADDRESS_PAN_MAX_SPEED, ftemp); + printi(F("Pan max speed: "), ftemp); + EEPROM.get(EEPROM_ADDRESS_TILT_MAX_SPEED, ftemp); + printi(F("Tilt max speed: "), ftemp); + EEPROM.get(EEPROM_ADDRESS_PAN_ACCELERATION, ftemp); + printi(F("Pan max acceleration: "), ftemp); + EEPROM.get(EEPROM_ADDRESS_TILT_ACCELERATION, ftemp); + printi(F("Tilt max acceleration: "), ftemp); + EEPROM.get(EEPROM_ADDRESS_HALL_PAN_OFFSET, ftemp); + printi(F("Pan Hall offset: "), ftemp); + EEPROM.get(EEPROM_ADDRESS_HALL_TILT_OFFSET, ftemp); + printi(F("Tilt Hall offset: "), ftemp); + printi(F("Pan invert: "), (byte)EEPROM.read(EEPROM_ADDRESS_INVERT_PAN)); + printi(F("Tilt invert: "), (byte)EEPROM.read(EEPROM_ADDRESS_INVERT_TILT)); + printi(F("Homing on start-up: "), (byte)EEPROM.read(EEPROM_ADDRESS_ENABLE_HOMING)); + printi(F("--------------------\n")); +} + +/*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +void setEEPROMVariables(void){ + printi(F("Setting values from EEPROM...\n")); + EEPROM.get(EEPROM_ADDRESS_MODE, step_mode); + EEPROM.get(EEPROM_ADDRESS_LIMIT_PAN_MIN, limit_pan_min); + EEPROM.get(EEPROM_ADDRESS_LIMIT_PAN_MAX, limit_pan_max); + EEPROM.get(EEPROM_ADDRESS_LIMIT_TILT_MIN, limit_tilt_min); + EEPROM.get(EEPROM_ADDRESS_LIMIT_TILT_MAX, limit_tilt_max); + EEPROM.get(EEPROM_ADDRESS_PAN_MAX_SPEED, pan_max_speed); + EEPROM.get(EEPROM_ADDRESS_TILT_MAX_SPEED, tilt_max_speed); + EEPROM.get(EEPROM_ADDRESS_PAN_ACCELERATION, pan_acceleration); + EEPROM.get(EEPROM_ADDRESS_TILT_ACCELERATION, tilt_acceleration); + EEPROM.get(EEPROM_ADDRESS_HALL_PAN_OFFSET, hall_pan_offset_degrees); + EEPROM.get(EEPROM_ADDRESS_HALL_TILT_OFFSET, hall_tilt_offset_degrees); + invert_pan = EEPROM.read(EEPROM_ADDRESS_INVERT_PAN); + invert_tilt = EEPROM.read(EEPROM_ADDRESS_INVERT_TILT); + enable_homing = EEPROM.read(EEPROM_ADDRESS_ENABLE_HOMING); + printi(F("Values set.\n")); +} + +/*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +void toggleAutoHoming(void){ + if(enable_homing == 0){ + enable_homing = 1; + printi(F("Homing enabled.\n")); + } + else{ + enable_homing = 0; + printi(F("Homing disabled.\n")); + } +} + +/*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +void serialData(void){ + char instruction = Serial.read(); + delay(2); //wait to make sure all data in the serial message has arived + memset(&stringText[0], 0, sizeof(stringText)); //clear the array + while(Serial.available()){//set elemetns of stringText to the serial values sent + char digit = Serial.read(); //read in a char + strncat(stringText, &digit, 1); //add digit to the end of the array + } + serialFlush();//Clear any excess data in the serial buffer + int serialCommandValueInt = atoi(stringText); //converts stringText to an int + float serialCommandValueFloat = atof(stringText); //converts stringText to a float + //if(command == 'G' && stringText[0] == 0) return;//For the bluetooth issue of it sending data when it connects... + switch(instruction){ + case INSTRUCTION_AUTO_HOME:{ + printi(F("Beginning homing...\n")); + if(findHome()){ + printi(F("Homing complete.\n")); + } + else{ + stepper_pan.setCurrentPosition(0); + stepper_tilt.setCurrentPosition(0); + setTargetPositions(0, 0); + printi(F("Error finding home position... Current position has been set as home.\n")); + } + } + break; + case INSTRUCTION_TOGGLE_HOMING:{ + toggleAutoHoming(); + } + break; + case INSTRUCTION_SET_PAN_HALL_OFFSET:{ + hall_pan_offset_degrees = serialCommandValueFloat; + } + break; + case INSTRUCTION_SET_TILT_HALL_OFFSET:{ + hall_tilt_offset_degrees = serialCommandValueFloat; + } + break; + case INSTRUCTION_INVERT_TILT:{ + invertTiltDirection(serialCommandValueInt); + } + break; + case INSTRUCTION_INVERT_PAN:{ + invertPanDirection(serialCommandValueInt); + } + break; + case INSTRUCTION_SAVE_TO_EEPROM:{ + printi(F("Saving values to EEPROM... \n")); + saveEEPROM(); + printi(F("Current valuses saved.\n")); + } + break; + case INSTRUCTION_ADD_POSITION:{ + addPosition(); + } + break; + case INSTRUCTION_STEP_FORWARD:{ + moveToIndex(current_moves_array_index + 1); + } + break; + case INSTRUCTION_STEP_BACKWARD:{ + moveToIndex(current_moves_array_index - 1); + } + break; + case INSTRUCTION_JUMP_TO_START:{ + gotoMovesArrayStart(); + } + break; + case INSTRUCTION_JUMP_TO_END:{ + gotoMovesArrayEnd(); + } + break; + case INSTRUCTION_EDIT_ARRAY:{ + editMovesArrayIndex(); + } + break; + case INSTRUCTION_ADD_DELAY:{ + addDelay(serialCommandValueInt); + } + break; + case INSTRUCTION_CLEAR_ARRAY:{ + clearArray(); + } + break; + case INSTRUCTION_EXECUTE_MOVES:{ + executeMoves(serialCommandValueInt); + } + break; + case INSTRUCTION_PAN_RUN_SPEED:{ + stepper_pan.setSpeed(panDegreesToSteps(serialCommandValueFloat)); + stepper_pan.runSpeed(); + } + break; + case INSTRUCTION_TILT_RUN_SPEED:{ + stepper_tilt.setSpeed(tiltDegreesToSteps(serialCommandValueFloat)); + stepper_tilt.runSpeed(); + } + break; + case INSTRUCTION_STATUS:{ + statusReport(); + } + break; + case INSTRUCTION_DEBUG_STATUS:{ + debugReport(); + } + break; + case INSTRUCTION_PAN_STEPS:{ + target_position[0] = serialCommandValueInt; + multi_stepper.moveTo(target_position); + } + break; + case INSTRUCTION_PAN_DEGREES:{ + panJogDegrees(serialCommandValueFloat); + } + break; + case INSTRUCTION_TILT_STEPS:{ + target_position[1] = serialCommandValueInt; + multi_stepper.moveTo(target_position); + } + break; + case INSTRUCTION_TILT_DEGREES:{ + tiltJogDegrees(serialCommandValueFloat); + } + break; +// case INSTRUCTION_HOME:{ +// target_position[0] = 0; +// target_position[1] = 0; +// multi_stepper.moveTo(target_position); //Sets new target positions +// } +// break; + case INSTRUCTION_SET_HOME:{ + stepper_pan.setCurrentPosition(0); + stepper_tilt.setCurrentPosition(0); + } + break; + case INSTRUCTION_ENABLE:{ + enableSteppers(serialCommandValueInt); + } + break; + case INSTRUCTION_STEP_MODE:{ + setStepMode(serialCommandValueInt); + } + break; + case INSTRUCTION_SET_ACCELLERATION:{ + printi("Setting acceleration to ", serialCommandValueFloat, 1, " steps/s/s.\n"); + pan_acceleration = serialCommandValueFloat; + tilt_acceleration = serialCommandValueFloat; + stepper_pan.setAcceleration(pan_acceleration); + stepper_tilt.setAcceleration(tilt_acceleration); + } + break; + case INSTRUCTION_SET_PAN_SPEED:{ + printi("Setting maximum pan speed to ", serialCommandValueFloat, 1, " steps/s.\n"); + pan_max_speed = serialCommandValueFloat; + stepper_pan.setMaxSpeed(pan_max_speed); + } + break; + case INSTRUCTION_SET_TILT_SPEED:{ + printi("Setting maximum tilt speed to ", serialCommandValueFloat, 1, " steps/s.\n"); + tilt_max_speed = serialCommandValueFloat; + stepper_tilt.setMaxSpeed(tilt_max_speed); + } + break; + case INSTRUCTION_MULTISTEPPER_TEST_2:{ + target_position[0] = panDegreesToSteps(45); + target_position[1] = tiltDegreesToSteps(180); + multi_stepper.moveTo(target_position); + } + break; + case INSTRUCTION_MULTISTEPPER_TEST_3:{ + target_position[0] = panDegreesToSteps(-45); + target_position[1] = tiltDegreesToSteps(-180); + multi_stepper.moveTo(target_position); + } + break; +// case COMMAND_CARTESIAN:{//Moves relitive to the current position +// while(Serial.available() != 6){//Wait for six bytes to be available. Breaks after ~200ms if bytes are not received. +// delayMicroseconds(200); +// count++; +// if(count > 1000){ +// serialFlush();//Clear the serial buffer +// break; +// } +// } +// int xTarget = (Serial.read() << 8) + Serial.read(); +// int yTarget = (Serial.read() << 8) + Serial.read(); +// int zTarget = (Serial.read() << 8) + Serial.read(); +// +// inverse_kinematics(end_effector.x + xTarget, end_effector.y + yTarget, end_effector.z + zTarget);//Calculates servo positions +// move_servos();//Moves the robot's servos +// } +// break; + } +} + +/*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +void mainLoop(void){ + while(1){ + if(Serial.available()) serialData(); + multi_stepper.run(); + } +} + +/*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ diff --git a/pan_tilt_mount_nano_code/panTiltMount.h b/pan_tilt_mount_nano_code/panTiltMount.h new file mode 100644 index 0000000..959faee --- /dev/null +++ b/pan_tilt_mount_nano_code/panTiltMount.h @@ -0,0 +1,137 @@ +#ifndef PANTILTMOUNT_H +#define PANTILTMOUNT_H + +/*------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +#define BAUD_RATE 57600 + +#define PIN_MS 11 +#define PIN_ENABLE 12 +#define PIN_DIRECTION_PAN 5 +#define PIN_STEP_PAN 6 +#define PIN_DIRECTION_TILT 7 +#define PIN_STEP_TILT 8 +#define PIN_DIRECTION_SLIDER 9 +#define PIN_STEP_SLIDER 10 +#define PIN_PAN_HALL A3 +#define PIN_TILT_HALL A4 +#define PIN_INPUT_VOLTAGE A5 + +#define FULL_STEP 1 +#define HALF_STEP 2 +#define QUARTER_STEP 4 +#define EIGHTH_STEP 8 +#define SIXTEENTH_STEP 16 + +#define PAN_GEAR_RATIO 8.4705882352941176470588235294118 //144/17 teeth +#define TILT_GEAR_RATIO 3.047619047619047619047619047619 //64/21 teeth + +#define MAX_STRING_LENGTH 10 +#define ARRAY_LENGTH 20 + +#define INSTRUCTION_STEP_MODE 'm' +#define INSTRUCTION_PAN_STEPS 'P' +#define INSTRUCTION_TILT_STEPS 'T' +#define INSTRUCTION_PAN_DEGREES 'p' +#define INSTRUCTION_TILT_DEGREES 't' +//#define INSTRUCTION_HOME 'h' +#define INSTRUCTION_SET_HOME 'h' +#define INSTRUCTION_ENABLE 'e' +#define INSTRUCTION_SET_ACCELLERATION 'a' +#define INSTRUCTION_SET_PAN_SPEED 's' +#define INSTRUCTION_SET_TILT_SPEED 'S' +#define INSTRUCTION_INVERT_PAN 'i' +#define INSTRUCTION_INVERT_TILT 'I' +#define INSTRUCTION_SET_PAN_HALL_OFFSET 'o' +#define INSTRUCTION_SET_TILT_HALL_OFFSET 'O' +#define INSTRUCTION_TOGGLE_HOMING 'H' + +#define INSTRUCTION_AUTO_HOME 'A' +#define INSTRUCTION_MULTISTEPPER_TEST_2 '2' +#define INSTRUCTION_MULTISTEPPER_TEST_3 '3' +#define INSTRUCTION_DEMO_1 '4' +#define INSTRUCTION_STATUS 'r' +#define INSTRUCTION_DEBUG_STATUS 'R' +#define INSTRUCTION_PAN_RUN_SPEED 'k' +#define INSTRUCTION_TILT_RUN_SPEED 'l' +#define INSTRUCTION_EXECUTE_MOVES ';' +#define INSTRUCTION_ADD_POSITION '#' +#define INSTRUCTION_STEP_FORWARD '>' +#define INSTRUCTION_STEP_BACKWARD '<' +#define INSTRUCTION_JUMP_TO_START '[' +#define INSTRUCTION_JUMP_TO_END ']' +#define INSTRUCTION_EDIT_ARRAY 'E' +#define INSTRUCTION_ADD_DELAY 'D' +#define INSTRUCTION_CLEAR_ARRAY 'C' + +#define INSTRUCTION_SAVE_TO_EEPROM 'U' + +#define EEPROM_ADDRESS_ENABLE_HOMING 0 +#define EEPROM_ADDRESS_LIMIT_PAN_MIN 1 +#define EEPROM_ADDRESS_LIMIT_PAN_MAX 5 +#define EEPROM_ADDRESS_LIMIT_TILT_MIN 9 +#define EEPROM_ADDRESS_LIMIT_TILT_MAX 13 +#define EEPROM_ADDRESS_PAN_MAX_SPEED 17 +#define EEPROM_ADDRESS_TILT_MAX_SPEED 21 +#define EEPROM_ADDRESS_PAN_ACCELERATION 25 +#define EEPROM_ADDRESS_TILT_ACCELERATION 29 +#define EEPROM_ADDRESS_HALL_PAN_OFFSET 33 +#define EEPROM_ADDRESS_HALL_TILT_OFFSET 37 +#define EEPROM_ADDRESS_INVERT_PAN 41 +#define EEPROM_ADDRESS_INVERT_TILT 42 +#define EEPROM_ADDRESS_MODE 43 + +#define VERSION_NUMBER "1.3.1" + +/*------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +struct ArrayElement { + long panStepCount = 0; + float panSpeed = 0; + long tiltStepCount = 0; + float tiltSpeed = 0; + int msDelay = 0; +}; + +/*------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +void initPanTilt(void); +void serialFlush(void); +void enableSteppers(bool); +void setStepMode(int); +void serialData(void); +void mainLoop(void); +void panJogDegrees(float); +void tiltJogDegrees(float); +float panDegreesToStep(float); +float tiltDegreesToStep(float); +void statusReport(void); +void debugReport(void); +bool findHome(void); +float getBatteryVoltage(void); +float getBatteryPercentage(void); +float boundFloat(float, float, float); +float panDegreesToSteps(float); +float tiltDegreesToSteps(float); +float panStepsToDegrees(long); +float tiltStepsToDegrees(long); +int addPosition(void); +void clearArray(void); +void executeMoves(int); +void moveToIndex(int); +void gotoMovesArrayStart(void); +void gotoMovesArrayEnd(void); +void editMovesArrayIndex(void); +void addDelay(unsigned int ms); +void printProgramElements(void); +void saveEEPROM(void); +void printEEPROM(void); +void setEEPROMVariables(void); +void invertPanDirection(bool); +void invertTiltDirection(bool); +int setTargetPositions(float, float); +void toggleAutoHoming(void); + +/*------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +#endif diff --git a/pan_tilt_mount_nano_code/pan_tilt_mount_nano_code.ino b/pan_tilt_mount_nano_code/pan_tilt_mount_nano_code.ino new file mode 100644 index 0000000..6f932e0 --- /dev/null +++ b/pan_tilt_mount_nano_code/pan_tilt_mount_nano_code.ino @@ -0,0 +1,41 @@ + /*-------------------------------------------------------------------------------------------------------------------------------------------------------- + * + * CHECK THE CODE FOR "TODO:" AND EDIT APPROPRIATELY + * + * The code is developed for a Delta robot. The robot is controlled by an Arduino Nano. + * + * Pan Tilt Mount STL files: + * + * Project video: + * + * All measurements are in SI units unless otherwise specified. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS + * IN THE SOFTWARE + * + * Code written by isaac879 + * + * Last modified: 23/03/2020 + * + *--------------------------------------------------------------------------------------------------------------------------------------------------------*/ +//TODOs: +//software limits +//point to point in x time +//status reports +//report commands + +/*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +#include "panTiltMount.h" + +/*--------------------------------------------------------------------------------------------------------------------------------------------------------*/ + +void setup(){ + initPanTilt(); +} + +void loop(){ + mainLoop(); +}