import pygame
import math
import serial
import RPi.GPIO as GPIO
from numpy import matrix, nan_to_num

# Define some colors
BLACK = (0, 0, 0)
WHITE = (255, 255, 255)

SERIAL_MARKER = 128
VECTOR_MOTORS_IDNT = 0
UP_DOWN_MOTOR_IDNT = 2
LIGHT_TOGGLE_IDNT = 5
CLAW_OPEN_CLOSE_IDNT = 4
CLAW_SPIN_IDNT = 3
CLAW_OPEN_CLOSE_GPIO_PIN = 2
TRIPPLE_ROTATE_RIGHT_IDNT = 6
TRIPPLE_ROTATE_LEFT_IDNT = 7

def pmap( value, istart, istop, ostart, ostop):
    return ostart + (ostop - ostart) * ((value - istart) / (istop - istart))
def sendSerial(serialPort , data):
    serialPort.write(data)

##### CONFIGURE T   HESE AXES #####
X_AXIS = 0
Y_AXIS = 1
YAW_AXIS = 4
INVERT_X = False