void updateQuad(struct MotorModel *motor) {
    unsigned char newQuadState = 0b00;
    signed long beforePosition = motor->position;

    if(*motor->sensorPort & motor->sensorAMask) {
        newQuadState |= 0b01;
    }
    if(*motor->sensorPort & motor->sensorBMask) {
        newQuadState |= 0b10;
    }

    //PORTBbits.RB5 = 0;

    if(motor->quadState != newQuadState) {
        switch(newQuadState) {
            case 0b00:
                    //PORTBbits.RB6 = 0;
                    //PORTBbits.RB7 = 0;
                    if(motor->quadState & 0b01) motor->position++;
                    if(motor->quadState & 0b10) motor->position--;
                break;
            case 0b01:
                    if(motor->quadState & 0b11) motor->position++;
                    if(motor->quadState & 0b00) motor->position--;
                    //PORTBbits.RB6 = 1;
                    //PORTBbits.RB7 = 0;
                break;
            case 0b10:
                    if(motor->quadState & 0b00) motor->position++;
                    if(motor->quadState & 0b11) motor->position--;
                    //PORTBbits.RB6 = 0;
                    //PORTBbits.RB7 = 1;
                break;
            case 0b11:
                    if(motor->quadState & 0b10) motor->position++;
                    if(motor->quadState & 0b01) motor->position--;
                    //PORTBbits.RB6 = 1;
                    //PORTBbits.RB7 = 1;
                break;
        }
        /*
        if(motor->position > beforePosition) {
            PORTBbits.RB7 = 0;
            PORTBbits.RB6 = 1;

        } else if(motor->position < beforePosition) {
            PORTBbits.RB7 = 1;
            PORTBbits.RB6 = 0;
        }
        */
        motor->quadState = newQuadState;
    }

}