/*
  PBotUnoServoMotors.cpp - eGizmo_PBot_Uno library
  Copyright (c) 2020 e-Gizmo Mechatronix Central Inc
  All right reserved.
*/
#include "eGizmo_PBot_Uno.h"


#define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4)
#define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4)

/* 
 * Servo Control
 * whichservo  - 1 to 4 ,ignore other values
 * pulsewidth  - 500 to 2499 (us)
 *		       - value less than 500 stops the servo PWM generator
 *		       - the pulsewidth converted to degrees from 0 to 180. 
 */

void  eGizmo_PBot_Uno::SERVO(uint8_t  whichservo, uint16_t position) {

  if((whichservo >= 1) && (whichservo <= 4)){
	if(position < MIN_PULSE_WIDTH){
		position = constrain(position, 0, 180);
		position = map(position,0,180,SERVO_MIN(), SERVO_MAX());
	}

	twi_send(25, whichservo + 0x1F, position, P_INT);
  }
}


void eGizmo_PBot_Uno::SETSERVO1(uint16_t minVal, uint16_t maxVal, uint16_t homeVal){

	if(minVal < MIN_PULSE_WIDTH){
		minVal = constrain(minVal, 0, 180);
		minVal = map(minVal,0,180,SERVO_MIN(), SERVO_MAX());
	}

	if(maxVal < MIN_PULSE_WIDTH){
		maxVal = constrain(maxVal, 0, 180);
		maxVal = map(maxVal,0,180,SERVO_MIN(), SERVO_MAX());
	}

	if(homeVal < MIN_PULSE_WIDTH){
		homeVal = constrain(homeVal, 0, 180);
		homeVal = map(homeVal,0,180,SERVO_MIN(), SERVO_MAX());
	}

	this->servo1Max = maxVal;
	this->servo1Min = minVal;
	this->servo1Home = homeVal;
}

void eGizmo_PBot_Uno::SETSERVO2(uint16_t minVal, uint16_t maxVal, uint16_t homeVal){

	if(minVal < MIN_PULSE_WIDTH){
		minVal = constrain(minVal, 0, 180);
		minVal = map(minVal,0,180,SERVO_MIN(), SERVO_MAX());
	}

	if(maxVal < MIN_PULSE_WIDTH){
		maxVal = constrain(maxVal, 0, 180);
		maxVal = map(maxVal,0,180,SERVO_MIN(), SERVO_MAX());
	}

	if(homeVal < MIN_PULSE_WIDTH){
		homeVal = constrain(homeVal, 0, 180);
		homeVal = map(homeVal,0,180,SERVO_MIN(), SERVO_MAX());
	}

	this->servo2Max = maxVal;
	this->servo2Min = minVal;
	this->servo2Home = homeVal;
}

void eGizmo_PBot_Uno::SETSERVO3(uint16_t minVal, uint16_t maxVal, uint16_t homeVal){

	if(minVal < MIN_PULSE_WIDTH){
		minVal = constrain(minVal, 0, 180);
		minVal = map(minVal,0,180,SERVO_MIN(), SERVO_MAX());
	}

	if(maxVal < MIN_PULSE_WIDTH){
		maxVal = constrain(maxVal, 0, 180);
		maxVal = map(maxVal,0,180,SERVO_MIN(), SERVO_MAX());
	}

	if(homeVal < MIN_PULSE_WIDTH){
		homeVal = constrain(homeVal, 0, 180);
		homeVal = map(homeVal,0,180,SERVO_MIN(), SERVO_MAX());
	}

	this->servo3Max = maxVal;
	this->servo3Min = minVal;
	this->servo3Home = homeVal;
}

void eGizmo_PBot_Uno::SETSERVO4(uint16_t minVal, uint16_t maxVal, uint16_t homeVal){

	if(minVal < MIN_PULSE_WIDTH){
		minVal = constrain(minVal, 0, 180);
		minVal = map(minVal,0,180,SERVO_MIN(), SERVO_MAX());
	}

	if(maxVal < MIN_PULSE_WIDTH){
		maxVal = constrain(maxVal, 0, 180);
		maxVal = map(maxVal,0,180,SERVO_MIN(), SERVO_MAX());
	}

	if(homeVal < MIN_PULSE_WIDTH){
		homeVal = constrain(homeVal, 0, 180);
		homeVal = map(homeVal,0,180,SERVO_MIN(), SERVO_MAX());
	}

	this->servo4Max = maxVal;
	this->servo4Min = minVal;
	this->servo4Home = homeVal;
}

void eGizmo_PBot_Uno::SERVO1(uint16_t position){

	if(position < MIN_PULSE_WIDTH){
		position = constrain(position, 0, 180);
		position = map(position,0,180,SERVO_MIN(), SERVO_MAX());
	}
	twi_send(25, 1 + 0x1F, position, P_INT);
}


void eGizmo_PBot_Uno::SERVO2(uint16_t position){

	if(position < MIN_PULSE_WIDTH){
		position = constrain(position, 0, 180);
		position = map(position,0,180,SERVO_MIN(), SERVO_MAX());
	}
	twi_send(25, 2 + 0x1F, position, P_INT);
}


void eGizmo_PBot_Uno::SERVO3(uint16_t position){

	if(position < MIN_PULSE_WIDTH){
		position = constrain(position, 0, 180);
		position = map(position,0,180,SERVO_MIN(), SERVO_MAX());
	}
	twi_send(25, 3 + 0x1F, position, P_INT);
}


void eGizmo_PBot_Uno::SERVO4(uint16_t position){

	if(position < MIN_PULSE_WIDTH){
		position = constrain(position, 0, 180);
		position = map(position,0,180,SERVO_MIN(), SERVO_MAX());
	}
	twi_send(25, 4 + 0x1F, position, P_INT);
}