# BBB_QC_Motor_Calibration.py
#
# Created by Peter Olejnik
#
# 			   Created on: 16/01/2016
# Last concious update on: 17/01/2016
#
# Purpose - This data collection program is an outright replacement for
#			the Quanser unit used up to this point. What this does is it
#			collectes reading from two ADCS, each hooked up to load
#			cells. In addition, it generates a throttle value between 0%
#			and 100% and sends out an appropriate PWM signal for the
# 			motor controller to use. All of this, as well as the time of
#			data being collected is all collected and stored within a 
#			CSV document. 
#
# Inputs -  Program wise none.
#
# Outputs - The ADC readout and pwm
#
#	   

############### Imports ###############

import Adafruit_BBIO.PWM as PWM # PWM functionality for the BBB
import Adafruit_BBIO.ADC as ADC # ADC functionality for the BBB
import BBB_QC					# Custom Libraries for the QC
import time   # time delay
import datetime # for extended time percision
import os     # identifies what the opperating system is
import sys    # related to the system. 
import signal # detects inputs
import csv    # adds csv functionality

############### Functions ###############

###### signal_handler ######
#
# Purpose - Detects if a keyboard inturupt is handled. If so, kills the 
#			program with a biut of clean up.
# 

def signal_handler(signal, frame):
	
	PWM.cleanup()
	print('Program halted! Exiting program!')
	sys.exit(0)

###### Main ######
#
# 

os.system('cls' if os.name == 'nt' else 'clear')

print 'Prepping ADC and PWM'

signal.signal(signal.SIGINT, signal_handler)

BBB_QC.Motor_Initialize()
time.sleep(0.1)
BBB_QC.Motor_All_Throttle(100)
time.sleep(0.1)
BBB_QC.Motor_All_Throttle(0)

Delay_Dat = 1


print "Set up Motor 1"
M1_CSV = open('M1.csv', 'a+')
raw_input("Press Enter when done.")

for Throttle in range(0,101,1):

	print 'Motor 1 Throttle at {0}%.'.format(Throttle)
	
#	BBB_QC.Motor_2_Throttle(Throttle)
	time.sleep(Delay_Dat)

	Thrust = []
	Torque = []
	EMF_Back = []
	
	Thrust.append(Throttle)
	Torque.append(Throttle)
	EMF_Back.append(Throttle)
	
	for point in range(0,100):
		
		Thrust.append(BBB_QC.Motor_2_ADC_Read())	# Used since ADC_In is P9_36
		Torque.append(BBB_QC.Motor_1_ADC_Read())	# Used since ADC_In is P9_38
		EMF_Back.append(BBB_QC.Motor_4_ADC_Read())	# Used since ADC_In is P9_40
		time.sleep(0.005)
		
	csv.writer(M1_CSV).writerow(Thrust)
	csv.writer(M1_CSV).writerow(Torque)
	csv.writer(M1_CSV).writerow(EMF_Back)
	

print "Set up Motor 3"
M1_CSV.close()
M3_CSV = open('M3.csv', 'a+')
raw_input("Press Enter when done.")

for Throttle in range(0,101,1):

	print 'Motor 3 Throttle at {0}%.'.format(Throttle)
	
#	BBB_QC.Motor_2_Throttle(Throttle)
	time.sleep(Delay_Dat)

	Thrust = []
	Torque = []
	EMF_Back = []
	
	Thrust.append(Throttle)
	Torque.append(Throttle)
	EMF_Back.append(Throttle)
	
	for point in range(0,100):
		
		Thrust.append(BBB_QC.Motor_2_ADC_Read())	# Used since ADC_In is P9_36
		Torque.append(BBB_QC.Motor_1_ADC_Read())	# Used since ADC_In is P9_38
		EMF_Back.append(BBB_QC.Motor_4_ADC_Read())	# Used since ADC_In is P9_40
		time.sleep(0.005)
		
	csv.writer(M3_CSV).writerow(Thrust)
	csv.writer(M3_CSV).writerow(Torque)
	csv.writer(M3_CSV).writerow(EMF_Back)


print "Set up Motor 2"
M3_CSV.close()
M2_CSV = open('M2.csv', 'a+')
raw_input("Press Enter when done.")

for Throttle in range(0,101,1):

	print 'Motor 2 Throttle at {0}%.'.format(Throttle)
	
#	BBB_QC.Motor_2_Throttle(Throttle)
	time.sleep(Delay_Dat)

	Thrust = []
	Torque = []
	EMF_Back = []
	
	Thrust.append(Throttle)
	Torque.append(Throttle)
	EMF_Back.append(Throttle)
	
	for point in range(0,100):
		
		Thrust.append(BBB_QC.Motor_2_ADC_Read())	# Used since ADC_In is P9_36
		Torque.append(BBB_QC.Motor_1_ADC_Read())	# Used since ADC_In is P9_38
		EMF_Back.append(BBB_QC.Motor_4_ADC_Read())	# Used since ADC_In is P9_40
		time.sleep(0.005)
		
	csv.writer(M2_CSV).writerow(Thrust)
	csv.writer(M2_CSV).writerow(Torque)
	csv.writer(M2_CSV).writerow(EMF_Back)

print "Set up Motor 4"
M2_CSV.close()
M4_CSV = open('M4.csv', 'a+')
raw_input("Press Enter when done.")

for Throttle in range(0,101,1):

	print 'Motor 4 Throttle at {0}%.'.format(Throttle)
	
#	BBB_QC.Motor_2_Throttle(Throttle)
	time.sleep(Delay_Dat)

	Thrust = []
	Torque = []
	EMF_Back = []
	
	Thrust.append(Throttle)
	Torque.append(Throttle)
	EMF_Back.append(Throttle)
	
	for point in range(0,100):
		
		Thrust.append(BBB_QC.Motor_2_ADC_Read())	# Used since ADC_In is P9_36
		Torque.append(BBB_QC.Motor_1_ADC_Read())	# Used since ADC_In is P9_38
		EMF_Back.append(BBB_QC.Motor_4_ADC_Read())	# Used since ADC_In is P9_40
		time.sleep(0.005)
		
	csv.writer(M4_CSV).writerow(Thrust)
	csv.writer(M4_CSV).writerow(Torque)
	csv.writer(M4_CSV).writerow(EMF_Back)

M4_CSV.close()
