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

/* 
 * Motor Controls 
 */

// Switch ON the motor driver
void  eGizmo_PBot_Uno::ALLON() {
  twi_send(25, TWI_MON, 0, P_NONE);
}

// switch off the motor driver (all motors)
void  eGizmo_PBot_Uno::ALLOFF() {
  twi_send(25, TWI_MOFF, 0, P_NONE);
}

/* 
 * set motor speed
 * whichmotor = MOTOR_A, MOTOR_B, or MOTOR_BOTH
 * speed = 0 to 255
 * 0 = full stop
 *
 * set motor direction
 * whichmotor = MOTOR_A, MOTOR_B, or MOTOR_BOTH
 * dir = MOTOR_FWD or MOTOR_REV
 *
 */

void eGizmo_PBot_Uno::FORWARD(uint8_t fwdSpeed){

	twi_send(25, TWI_MFWDA, 0, P_NONE);
	twi_send(25, TWI_MFWDB, 0, P_NONE);
	twi_send(25, TWI_MSPEEDA, fwdSpeed, P_BYTE);
	twi_send(25, TWI_MSPEEDB, fwdSpeed, P_BYTE);
	//break;
}

void eGizmo_PBot_Uno::FORWARD(uint8_t Lspeed, uint8_t Rspeed){

	twi_send(25, TWI_MFWDA, 0, P_NONE);
	twi_send(25, TWI_MFWDB, 0, P_NONE);
	twi_send(25, TWI_MSPEEDA, Lspeed, P_BYTE);
	twi_send(25, TWI_MSPEEDB, Rspeed, P_BYTE);
	//break;
}

void eGizmo_PBot_Uno::REVERSE(uint8_t revSpeed){

	revSpeed = ~revSpeed;
	twi_send(25, TWI_MREVA, 0, P_NONE);
	twi_send(25, TWI_MREVB, 0, P_NONE);
	twi_send(25, TWI_MSPEEDA, revSpeed, P_BYTE);
	twi_send(25, TWI_MSPEEDB, revSpeed, P_BYTE);
	//break;
}

void eGizmo_PBot_Uno::REVERSE(uint8_t Lspeed, uint8_t Rspeed){
	
	twi_send(25, TWI_MREVA, 0, P_NONE);
	twi_send(25, TWI_MREVB, 0, P_NONE);
	twi_send(25, TWI_MSPEEDA, ~Lspeed, P_BYTE);
	twi_send(25, TWI_MSPEEDB, ~Rspeed, P_BYTE);
}

void eGizmo_PBot_Uno::STOP(void){

	//fullstop = 0;
	twi_send(25, TWI_MFWDA, 0, P_NONE);
	twi_send(25, TWI_MFWDB, 0, P_NONE);
	twi_send(25, TWI_MSPEEDA, 0, P_BYTE);
	twi_send(25, TWI_MSPEEDB, 0, P_BYTE);
}

void eGizmo_PBot_Uno::TURNLEFT(uint8_t Rspeed){

	twi_send(25, TWI_MFWDA, 0, P_NONE);
	twi_send(25, TWI_MFWDB, 0, P_NONE);
	twi_send(25, TWI_MSPEEDA, 0, P_BYTE);
	twi_send(25, TWI_MSPEEDB, Rspeed, P_BYTE);
}

void eGizmo_PBot_Uno::TURNRIGHT(uint8_t Lspeed){
	
	twi_send(25, TWI_MFWDA, 0, P_NONE);
	twi_send(25, TWI_MFWDB, 0, P_NONE);
	twi_send(25, TWI_MSPEEDA, Lspeed, P_BYTE);
	twi_send(25, TWI_MSPEEDB, 0, P_BYTE);
}

void eGizmo_PBot_Uno::SPINRIGHT(uint8_t spinSpeed){

	twi_send(25, TWI_MFWDA, 0, P_NONE);
	twi_send(25, TWI_MREVB, 0, P_NONE);
	twi_send(25, TWI_MSPEEDA, spinSpeed, P_BYTE);
	twi_send(25, TWI_MSPEEDB, ~spinSpeed, P_BYTE);
}

void eGizmo_PBot_Uno::SPINLEFT(uint8_t spinSpeed){
	
	twi_send(25, TWI_MREVA, 0, P_NONE);
	twi_send(25, TWI_MFWDB, 0, P_NONE);
	twi_send(25, TWI_MSPEEDA, ~spinSpeed, P_BYTE);
	twi_send(25, TWI_MSPEEDB, spinSpeed, P_BYTE);
}