/* Purpose Period (ms) Duty (ms) Prescaler Period Duty -------------------------------------------------------------------------------- LED throb 0.020 0–0.020 1 (none) 65535 (default) 0–767 Servo control 20 1.25 (0°) 21 65535 (default) 4096 1.50 (90°) 21 65535 (default) 4915 1.75 (180°) 21 65535 (default) 5734 imamo 1638 koraka max rezolucije za vreme izmedju 1.25us do 1.75us, toliko ce mo i impulsa da imamo za jednu osu prepravljeno na 2000 rezoluciju */ char myInfo[] = "/------------------------------------\\\n" "| 4 channel Step/Dir -> PWM converter |\n" "| Type 'h' for help |\n" "\\------------------------------------/\n" ; char myHelp[] = "/------------------------------------\\\n" "Command overview \n" "h H - Help\n" "m M - Print MIN and MAX axis values\n" "p P - Print PWM settings\n" "z Z - Zero all axis to 50%\n" "q Q - Show Input/Output Pins mapping\n" "0 space - Print current axis position\n" "1x - Set X axis position where x is string 0 - 9\n" "2x - Set Y axis position where x is string 0 - 9\n" "3x - Set Z axis position where x is string 0 - 9\n" "4x - Set A axis position where x is string 0 - 9\n" ; volatile int state = 0; // must declare volatile, since it's // modified within the blink() handler volatile int cnt; char myStr[32]; //----------------------------------------- //definicija za 4 ose STEP/DIR -> SERVO PWM //duty i vremena za servo (slicno) kao iz tablice #define pwm_duty_half 5100 //1.5ms #define pwm_duty_delta 3000 #define pwm_duty_min (pwm_duty_half - pwm_duty_delta / 2) //4096 #define pwm_duty_max (pwm_duty_half + pwm_duty_delta / 2) //5734 //lmiti osa int axis_x_min = 1; int axis_y_min = 1; int axis_z_min = 1; int axis_a_min = 1; int axis_x_max = pwm_duty_delta; int axis_y_max = pwm_duty_delta; int axis_z_max = pwm_duty_delta; int axis_a_max = pwm_duty_delta; int axis_x_half = axis_x_max / 2; int axis_y_half = axis_y_max / 2; int axis_z_half = axis_z_max / 2; int axis_a_half = axis_a_max / 2; //definicija i pocetno stanje osa volatile int axis_x_pos = pwm_duty_delta / 2; volatile int axis_y_pos = pwm_duty_delta / 2; volatile int axis_z_pos = pwm_duty_delta / 2; volatile int axis_a_pos = pwm_duty_delta / 2; volatile bool axis_update = true; //definicija naziva pinova za STEP/DIR #define PIN_STEP_X 31 #define PIN_DIR_X 30 #define PIN_STEP_Y 29 #define PIN_DIR_Y 28 #define PIN_STEP_Z 22 #define PIN_DIR_Z 21 #define PIN_STEP_A 20 // <<<<<<<<<< #define PIN_DIR_A 19 // <<<<<<<<<<<< //definicija naziva pinova SERVO PWM #define PIN_PWM_X 8 #define PIN_PWM_Y 9 #define PIN_PWM_Z 10 #define PIN_PWM_A 11 // Use timer2 - this timer is used for PWMs on pin 8, 9, 10, 11 HardwareTimer timer(2); bool debug = false; //----------------------------------------- // SETUP //----------------------------------------- void setup() { // Set up the built-in LED pin as an output and button: pinMode(BOARD_LED_PIN, OUTPUT); pinMode(BOARD_BUTTON_PIN, INPUT); //definicija za STEP/DIR ulaze (INPUT | INPUT_PULLUP) pinMode(PIN_STEP_X, INPUT ); pinMode(PIN_STEP_Y, INPUT ); pinMode(PIN_STEP_Z, INPUT ); pinMode(PIN_STEP_A, INPUT ); pinMode(PIN_DIR_X, INPUT); pinMode(PIN_DIR_Y, INPUT); pinMode(PIN_DIR_Z, INPUT); pinMode(PIN_DIR_A, INPUT); //definicija za PWM izlaze timer.setPrescaleFactor(21); //za 50Hz RC servo pinMode(PIN_PWM_X, PWM); pinMode(PIN_PWM_Y, PWM); pinMode(PIN_PWM_Z, PWM); pinMode(PIN_PWM_A, PWM); //vezi interapt rutine na STEP pinove attachInterrupt(PIN_STEP_X, update_x, RISING); attachInterrupt(PIN_STEP_Y, update_y, RISING); attachInterrupt(PIN_STEP_Z, update_z, RISING); attachInterrupt(PIN_STEP_A, update_a, RISING); //ukljuci debug mode preko USB ako je prilikom boot pritisnut taster if (digitalRead(BOARD_BUTTON_PIN)) { debug = true; } } //----------------------------------------- // LOOP //----------------------------------------- void loop() { //some counter cnt++; if(SerialUSB.isConnected() && (SerialUSB.getDTR() || SerialUSB.getRTS())) { cnt++;cnt++; } if (cnt > 65535) { cnt = 0; toggleLED(); } //------------- //axis update if (axis_update == true) { saturate_all_pos(0); //updejtuj PWM pwmWrite(PIN_PWM_X, pwm_duty_min + axis_x_pos); pwmWrite(PIN_PWM_Y, pwm_duty_min + axis_y_pos); pwmWrite(PIN_PWM_Z, pwm_duty_min + axis_z_pos); pwmWrite(PIN_PWM_A, pwm_duty_min + axis_a_pos); axis_update = false; } //--------------- //read from USB int aval = SerialUSB.available(); if (aval) { for (int i=0; i < aval; i++) { myStr[i] = SerialUSB.read(); } for (int i=aval; i < 32; i++) { myStr[i] = '\0'; } //echo back SerialUSB.print(">"); SerialUSB.println(myStr); //call decoder decode_command(); } //ako je pritisnut taster resetuj PWM na 50% if (digitalRead(BOARD_BUTTON_PIN)) { zero_pos (); } } //----------------------------------------- //interapt rutina za update X brojaca //----------------------------------------- void update_x () { if (digitalRead(PIN_DIR_X)) axis_x_pos++; else axis_x_pos--; axis_update = true; } //----------------------------------------- //interapt rutina za update Y brojaca //----------------------------------------- void update_y () { if (digitalRead(PIN_DIR_Y)) axis_y_pos++; else axis_y_pos--; axis_update = true; } //----------------------------------------- //interapt rutina za update Z brojaca //----------------------------------------- void update_z () { if (digitalRead(PIN_DIR_Z)) axis_z_pos++; else axis_z_pos--; axis_update = true; } //----------------------------------------- //interapt rutina za update A brojaca //----------------------------------------- void update_a () { if (digitalRead(PIN_DIR_A)) axis_a_pos++; else axis_a_pos--; axis_update = true; } //----------------------------------------- //DUMP //----------------------------------------- void dump_vars (int type) { if(SerialUSB.isConnected() && (SerialUSB.getDTR() || SerialUSB.getRTS())) { if (type == 0) { //status SerialUSB.println("------------------------------"); SerialUSB.println(" x | y | z | a"); SerialUSB.print(axis_x_pos); SerialUSB.print(" | "); SerialUSB.print(axis_y_pos); SerialUSB.print(" | "); SerialUSB.print(axis_z_pos); SerialUSB.print(" | "); SerialUSB.print(axis_a_pos); SerialUSB.println(""); } if (type == 1) { //min max SerialUSB.println("------------------------------"); SerialUSB.println(" x-min | y-min |z-min | a-min"); SerialUSB.print(axis_x_min,4); SerialUSB.print(" | "); SerialUSB.print(axis_y_min); SerialUSB.print(" | "); SerialUSB.print(axis_z_min); SerialUSB.print(" | "); SerialUSB.print(axis_a_min); SerialUSB.println(""); SerialUSB.println("------------------------------"); SerialUSB.println( "x-max | y-max | z-max | a-max"); SerialUSB.print(axis_x_max); SerialUSB.print(" | "); SerialUSB.print(axis_y_max); SerialUSB.print(" | "); SerialUSB.print(axis_z_max); SerialUSB.print(" | "); SerialUSB.print(axis_a_max); SerialUSB.println(""); } if (type == 2) { //duty podaci SerialUSB.println("------------------------------"); SerialUSB.print ("pwm_duty_half (1.5ms): "); SerialUSB.println(pwm_duty_half); SerialUSB.print ("pwm_duty_delta: "); SerialUSB.println(pwm_duty_delta); SerialUSB.print ("pwm_duty_min: "); SerialUSB.println(pwm_duty_min); SerialUSB.print ("pwm_duty_max: "); SerialUSB.println(pwm_duty_max); } if (type == 3) { //definicija naziva pinova za STEP/DIR SerialUSB.println("------------------------------"); SerialUSB.print ("PIN_STEP_X IN: "); SerialUSB.println(PIN_STEP_X); SerialUSB.print ("PIN_DIR_X IN: "); SerialUSB.println(PIN_DIR_X); SerialUSB.print ("PIN_STEP_Y IN: "); SerialUSB.println(PIN_STEP_Y); SerialUSB.print ("PIN_DIR_Y IN: "); SerialUSB.println(PIN_DIR_Y); SerialUSB.print ("PIN_STEP_Z IN: "); SerialUSB.println(PIN_STEP_Z); SerialUSB.print ("PIN_DIR_Z IN: "); SerialUSB.println(PIN_DIR_Y); SerialUSB.print ("PIN_STEP_A IN: "); SerialUSB.println(PIN_STEP_A); SerialUSB.print ("PIN_DIR_A IN: "); SerialUSB.println(PIN_DIR_X); SerialUSB.print ("PIN_PWM_X OUT: "); SerialUSB.println(PIN_PWM_X); SerialUSB.print ("PIN_PWM_Y OUT: "); SerialUSB.println(PIN_PWM_Y); SerialUSB.print ("PIN_PWM_Z OUT: "); SerialUSB.println(PIN_PWM_Z); SerialUSB.print ("PIN_PWM_A OUT: "); SerialUSB.println(PIN_PWM_A); } } } //----------------------------------------- //Command decoder ************************************* //----------------------------------------- void decode_command() { //dekodiramo samo prva dva karaktera int axis = int(myStr[0]); //1,2,3,4 int pos = int(myStr[1]); //0 ... B switch (axis) { case 32: //ascii , dump vars - curent axis status case 48: //ascii 0, dump vars - curent axis status dump_vars (0); break; case 49: //ascii 1, set X axis_x_pos = (axis_x_max / 10) * (pos - 48); SerialUSB.println("ok"); break; case 50: //ascii 2, set Y axis_y_pos = (axis_y_max / 10) * (pos - 48); SerialUSB.println("ok"); break; case 51: //asci 3, set Z axis_z_pos = (axis_z_max / 10) * (pos - 48); SerialUSB.println("ok"); break; case 52: //asci 4, set A axis_a_pos = (axis_a_max / 10) * (pos - 48); SerialUSB.println("ok"); break; case 122: //asci z, zero position case 90: //asci Z, zero position zero_pos (); SerialUSB.println("ok"); break; case 109: //asci m, min/max position case 77: //asci M, min/max position dump_vars (1); break; case 112: //asci p, PWM info case 80: //asci P, PWM info dump_vars (2); break; case 113: //asci q, PIN I/O info case 81: //asci Q, PIN I/O info dump_vars (3); break; case 104: //asci h, help case 72: //asci H, help SerialUSB.println(myHelp); break; default: // if nothing else matches, do the default SerialUSB.println("err"); SerialUSB.println(myInfo); break; } //saturate_all_pos(0); axis_update = true; } //----------------------------------------- //saturacija //----------------------------------------- void saturate_all_pos (int axis) { if (axis==1 || axis==0){ if (axis_x_pos < axis_x_min) axis_x_pos = axis_x_min; if (axis_x_pos > axis_x_max) axis_x_pos = axis_x_max; } if (axis==2 || axis==0){ if (axis_y_pos < axis_y_min) axis_y_pos = axis_y_min; if (axis_y_pos > axis_y_max) axis_y_pos = axis_y_max; } if (axis==3 || axis==0){ if (axis_z_pos < axis_z_min) axis_z_pos = axis_z_min; if (axis_z_pos > axis_z_max) axis_z_pos = axis_z_max; } if (axis==4 || axis==0){ if (axis_a_pos < axis_a_min) axis_a_pos = axis_a_min; if (axis_a_pos > axis_a_max) axis_a_pos = axis_a_max; } } //----------------------------------------- //zero //----------------------------------------- void zero_pos () { pwmWrite(PIN_PWM_X, pwm_duty_half); pwmWrite(PIN_PWM_Y, pwm_duty_half); pwmWrite(PIN_PWM_Z, pwm_duty_half); pwmWrite(PIN_PWM_A, pwm_duty_half); axis_x_pos = axis_x_half; axis_y_pos = axis_x_half; axis_z_pos = axis_x_half; axis_a_pos = axis_x_half; }