#include <xc.h>
#include "pins.h"
#include "motor.h"



// 1 tour = 2 CPR
#define TURN2TICK (2)

int16_t mot_curPos = 0;
int8_t oldPOS;


void mot_init(void) {
    mot_curPos = 0;
    oldPOS = POS;
}


void mot_down_turn(uint8_t dist_mm) {
    int16_t endPos;
    
    endPos = mot_curPos - dist_mm*TURN2TICK;  // dist are positive upwards
    ACTIVE = 1;     // activate encoders
    mot_down();
    while ( mot_curPos >= endPos) {
        if ( POS != oldPOS ) {
            mot_curPos -= 1;
            oldPOS = POS;
        }
    }
    mot_stop(); // stop the motor
    ACTIVE = 0; // deactivate encoders
}


void mot_up_turn(uint8_t dist_mm) {
    int16_t endPos;
    
    endPos = mot_curPos + dist_mm*TURN2TICK;  // dist are positive upwards
    ACTIVE = 1;     // activate encoders
    mot_up();       // start the motor
    while ( mot_curPos <= endPos) {
        if ( POS != oldPOS ) {
            mot_curPos += 1;
            oldPOS = POS;
        }
    }
    mot_stop(); // stop the motor
    ACTIVE = 0; // deactivate encoders
}


void mot_delay(void) {
    for (uint8_t i=0; i<200; i++);
}


void mot_stop(void) {
    COM1 = 1;
    COM2 = 1;
    COM3 = 0;
    COM4 = 0;
    mot_delay();
}


void mot_up(void) {
    // turn off up diagonal
    COM2 = 1;
    COM3 = 0;
    // delay to avoid short-circuits
    mot_delay();
    // turn on down diagonal
    COM1 = 0;
    COM4 = 1;
}


void mot_down(void) {
    // turn off down diagonal
    COM1 = 1;
    COM4 = 0;
    // delay to avoid short-circuits
    mot_delay();
    // turn on down diagonal
    COM2 = 0;
    COM3 = 1;
}