#!/usr/bin/env python

#based on servo.py by Brian D. Wendt http://principialabs.com/

#####################################
# Module:   servo.py
# Created:  24 Aug 2012
# Author:   Patrick R. LeClair
#   http://bama.ua.edu/~pleclair
# Version:  0.1
# License:  GPLv3
#   http://www.fsf.org/licensing/
'''
Provides a serial connection abstraction layer
for use with an Arduino sketch that controls
a continuous rotation servo. While rotating,
monitor a sensor on A0.
Save CSV file of (step,sensor) data and plot
result when completed

usage:  servo.measure(servo number, speed[0-100], steps)
        servo.move(servo number, speed, steps)
'''
################################################

import serial
import csv
import time
from pylab import *

# Assign Arduino's serial port address
#   Windows example     usbport = 'COM3'
#   Linux example     usbport = '/dev/ttyUSB0'
#   MacOSX example     usbport = '/dev/tty.usbserial-FTALLOK2'
usbport = 'COM7'

# Set up serial baud rate
ser = serial.Serial(usbport, 9600, timeout=1)

def get_number():
        num=ser.readline()
        try:
            return(int(num))
        except:
            return(0)
        
def measure(servo, speed, steps):
    '''Moves the specified cont. rotation servo at a given speed
        through the desired number of steps. Reads a sensor on A0
        at the same time and spits (step,sensor) data to a CSV file

    Arguments:
        servo
          the servo number to command, an integer from 1-4
        speed
          the relative speed, from 0 to 100
        steps
          how many steps to move through'''

    data = csv.writer(open('foo.csv','wb',buffering=0),delimiter=",")
    y=[]
    x=[]
    temp=[]
    intensity=0

    if (0 <= speed <= 100):
        for i in range(steps):        
            ser.write(chr(255))
            ser.write(chr(servo))
            ser.write(chr(speed))
            intensity=get_number()
            data.writerow([i,intensity])
            x.append(i)
            y.append(intensity)
        plot(x,y)
        xlabel('angle (steps)')
        ylabel('LDR voltage (V)')
        title(r'LDR voltage versus angle')
        show()
    else:
        print "Servo angle must be an integer between 0 and 100.\n"
CLOSE THE FILE ... with a try/finally to be sure?

def move(servo, speed, steps):
    '''Moves the specified cont. rotation servo at a given speed
        through the desired number of steps. useful for getting
        to starting position

    Arguments:
        servo
          the servo number to command, an integer from 1-4
        speed
          the relative speed, from 0 to 100
        steps
          how many steps to move through'''

    if (0 <= speed <= 100):
        for i in range(steps):        
            ser.write(chr(255))
            ser.write(chr(servo))
            ser.write(chr(speed))
    else:
        print "Servo angle must be an integer between 0 and 100.\n"