looks nice, but still wrong

git-svn-id: https://svn.spreadspace.org/mur.sat@322 7de4ea59-55d0-425e-a1af-a3118ea81d4c

1 files changed, 72 insertions, 29 deletions

diff --git a/tools/photodiodesim/pd_sim.py b/tools/photodiodesim/pd_sim.py
index abfca48..a5f25b9 100755
--- a/tools/photodiodesim/pd_sim.py
+++ b/tools/photodiodesim/pd_sim.py
@@ -3,33 +3,45 @@
 """
 Created on Tue Mar 20 21:18:28 2012
 
-@author: bernhard
+@author: Bernhard Tittelbach, Othmar Gsenger
 """
 
-import math
 import numpy as np
 import pylab as pl
+import scipy.io
+import re
 
-simduration = 60 #s
+simduration = 300 #s
 simtimestep = 0.1 #s
-w_x = 0.1 #rad/s
-w_y = 0.2 #rad/s
-w_z = 0.5 #rad/s
+w_x = np.random.ranf() * np.pi/4 #rad/s
+w_y = np.random.ranf() * np.pi/4 #rad/s
+w_z = np.random.ranf() * np.pi/4 #rad/s
+filename_export_mat = "./pd_sim.mat"
+filename_export_csv =  "./pd_sim.csv"
+
+
+print "Simulation Input:"
+print "　 Duration:", simduration
+print "　 Step Size", simtimestep
+print "　 Angular Speed x-axis [rad/s]", w_x
+print "　 Angular Speed y-axis [rad/s]", w_y
+print "　 Angular Speed z-axis [rad/s]", w_z
 
-#todo simloop mit random werten
 
 simtimeline = np.arange(0,simduration,simtimestep)
 
 w_xyz = np.matrix([w_x,w_y,w_z])
 
-torque_sum_vectors = ( w_xyz.transpose() * simtimeline ).transpose() % (2 *math.pi)
+angular_speed_sum_vectors = ( w_xyz.transpose() * simtimeline ).transpose() % (2 *np.pi)
 
-sensors = np.matrix([[ 1,0,0],
+sensors = np.matrix([
+                    [ 1,0,0],
+                    [ -1,0,0],
                     [0,1,0],
-                    [0,0,1],
-                    [-1,0,0],
                     [0,-1,0],
-                    [0,0,-1]])
+                    [0,0,1],
+                    [0,0,-1]
+                    ])
 
 sun= np.matrix([ 1,0,0])
 
@@ -38,30 +50,41 @@ def getRotationMatrix(angles):
     y_angle = angles[0,1]
     z_angle = angles[0,2]
     return np.matrix([
-    [1,0,0],
-    [0,math.cos(x_angle),math.sin(x_angle)],
-    [0,math.sin(x_angle),math.cos(x_angle)]]) * np.matrix(
-        [ [math.cos(y_angle),0,math.sin(y_angle)],
-                [0,1,0],
-                [math.sin(y_angle),0,math.cos(y_angle)]]
-         ) *   np.matrix([
-         [math.cos(z_angle),math.sin(z_angle),0],
-                 [math.sin(z_angle),math.cos(z_angle),0],
-                  [0,0,1]
-        ])
+      [1,   0,                  0],
+      [0,   np.cos(x_angle),    -np.sin(x_angle)],
+      [0,   np.sin(x_angle),    np.cos(x_angle)]
+    ]) * np.matrix([
+      [np.cos(y_angle) ,0,np.sin(y_angle)],
+      [0,               1,0],
+      [-np.sin(y_angle),0,np.cos(y_angle)]
+    ]) * np.matrix([
+      [np.cos(z_angle),-np.sin(z_angle),0],
+      [np.sin(z_angle), np.cos(z_angle),0],
+      [0,0,1]
+    ])
 
 def rotateVector(vec_to_rotate, angles_vector):
     return vec_to_rotate * getRotationMatrix(angles_vector)
 
 def sensor_model_rel_sensitivity_cos(angle):
-    if angle < math.pi/2.0:
+    if angle < np.pi/2.0:
         return np.cos(angle[0,0])
     else:
         return 0.0
 
+def _quote(str):
+    return '"'+ re.sub(r'(?!\\)"','\\"',str)  +'"'
+        
+def exportCSV(dst_file_path, names, tline, smatrix):
+    with open(dst_file_path, "w") as fh:
+        fh.truncate()
+        fh.write(";".join(map(_quote, ["SimTime"]+names)) +"\n")
+        for row in np.vstack((tline,smatrix.T)).T:
+            fh.write(";".join(["%f" % x for x in row.T]) +"\n")
+
 phicostrace=[]
 senstrace=[]
-for cur_rot in torque_sum_vectors:
+for cur_rot in angular_speed_sum_vectors:
     sens1=rotateVector(sensors[0],cur_rot)
     rot_sensors=rotateVector(sensors,cur_rot)
     #sinphi=np.cross(sens1,sun) # /1 / 1
@@ -75,8 +98,28 @@ for cur_rot in torque_sum_vectors:
     phicostrace.append(angle)
     senstrace.append([sensor_model_rel_sensitivity_cos(a) for a in angle])
 
-#print senstrace
-#print np.matrix(senstrace).transpose()
-pl.plot(simtimeline, senstrace)
-pl.legend(["Sensor"+str(x) for x in range(1,7)])
+senstrace = np.matrix(senstrace)
+legend_names=["Sensor%d %s" % (x+1, sensors[x]) for x in range(0,6)]
+
+print "Saving to " + filename_export_mat
+scipy.io.savemat(filename_export_mat, mdict={"legend":legend_names, "senstrace":senstrace,"simtimeline":simtimeline,"angular_speed_xyz":w_xyz}, oned_as="column")
+print "Saving to " + filename_export_csv
+exportCSV(filename_export_csv, legend_names, simtimeline, senstrace)
+
+pl.figure()
+for f in range(0,3):
+  fs = 2*f
+  fe = fs + 2
+  pl.subplot(3,1,f+1)
+  pl.plot(simtimeline, senstrace[:,fs:fe])
+  pl.legend(legend_names[fs:fe])
+
+pl.figure()
+spncols=2
+spnrows=3
+for s in range(0,6):
+  pl.subplot(spnrows,spncols,s+1)
+  pl.plot(simtimeline, senstrace[:,s])
+  pl.title(legend_names[s])
+
 pl.show()