// #include /* Blink Turns on the built-in LED on for 0.1 second, then off for one second, repeatedly. 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 */ volatile int state = 0; // must declare volatile, since it's // modified within the blink() handler int cnt; char myStr[32]; //----------------------------------------- //definicija za 3 ose STEP/DIR -> SERVO PWM //duty i vremena za servo 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; //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 9 #define PIN_PWM_Y 10 #define PIN_PWM_Z 11 #define PIN_PWM_A 8 //<<<<<<<<<<<<< // 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: pinMode(BOARD_LED_PIN, OUTPUT); pinMode(BOARD_BUTTON_PIN, INPUT); //SerialUSB.println("Welcome to the machine!"); //definicija za STEP/DIR ulaze (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; } //za probu attachInterrupt(BOARD_BUTTON_PIN, blink, RISING); } //----------------------------------------- // LOOP //----------------------------------------- void loop() { if(SerialUSB.isConnected() && (SerialUSB.getDTR() || SerialUSB.getRTS())) { digitalWrite(BOARD_LED_PIN, 1); debug = 1; } else { digitalWrite(BOARD_LED_PIN, 0); debug = 0; } cnt++; if (cnt > 65535) cnt = 0; //toggleLED(); // Turn the LED from off to on, or on to off //if (debug) delay(100); // Wait for 0.1 second (100 milliseconds) if (axis_update == true) { saturate_all_pos(0); //updejtuj PWM //pwmWrite(BOARD_LED_PIN, 654 * axis_x_pos); 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); if (debug) { //dump_vars (); } dump_vars (); 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'; } SerialUSB.print("STIGLO:"); SerialUSB.print(myStr); SerialUSB.println("."); decode_command(); } //ako je pritisnut taster resetuj PWM na 50% if (digitalRead(BOARD_BUTTON_PIN)) { 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_max / 2; axis_y_pos = axis_y_max / 2; axis_z_pos = axis_z_max / 2; axis_a_pos = axis_a_max / 2; } } //za probu interapta void blink() { state++; } //----------------------------------------- //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 () { if(SerialUSB.isConnected() && (SerialUSB.getDTR() || SerialUSB.getRTS())) { SerialUSB.println("----------------------------------------"); SerialUSB.println("cnt | x | y | z | a | maxX | state"); SerialUSB.println("----------------------------------------"); SerialUSB.print(cnt); SerialUSB.print(" | "); 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.print(" | "); SerialUSB.print(axis_x_max); SerialUSB.print(" | "); SerialUSB.println(state); } } //----------------------------------------- //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 49: //ascii 1 axis_x_pos = (axis_x_max / 10) * (pos - 48); break; case 50: //ascii 2 axis_y_pos = (axis_y_max / 10) * (pos - 48); break; case 51: //asci 3 axis_z_pos = (axis_z_max / 10) * (pos - 48); break; case 52: //asci 4 axis_a_pos = (axis_a_max / 10) * (pos - 48); break; default: // if nothing else matches, do the default // default is optional SerialUSB.println("err"); 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; } }