-
-
Notifications
You must be signed in to change notification settings - Fork 2
/
Saddle_generator.py
72 lines (54 loc) · 1.93 KB
/
Saddle_generator.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
import math
import numpy
import matplotlib.pyplot as plt
from scipy import interpolate
import csv
numpy.set_printoptions(suppress=True)
# Change these values depending on you motor and encoder
encoder_res = 1024
motor_poles = 21 # for 24N22P use 11 for 36N42P use 21 for 11N14P use 7
# Variable to adjust peak value of saddle to number we want
# Usually peak must be 2 * A
var = 1039
# Peak value of saddle = A * 2
# Value is determined by pwm resolution on you MCU
# Here 2*A would be 100% duty cycle in our MCU
A = 900
sample = 1000
step = (2 * math.pi) / (sample)
a1 = list(numpy.arange(0,2*math.pi,step))
a1.append(math.pi)
sin1 = [var * math.sin(n) for n in a1]
sin2 = [var * math.sin(n-((2*math.pi)/3)) for n in a1]
sin3 = [var * math.sin(n+((2*math.pi)/3)) for n in a1]
phase = []
for n in range(sample+1):
temp = (min(sin1[n],sin2[n],sin3[n]) + max(sin1[n],sin2[n],sin3[n])) /2;
phase.append(sin1[n] - temp + A )
n = int(motor_poles)
print(f"We need {n} periods for whole rotation")
y = phase * n
#plt.plot(y)
#plt.show()
x = range(1,n*len(phase)+1,1)
print(f"We have {len(x)} elements in old wave")
new_x = numpy.linspace(min(x),max(x),encoder_res)
print(f"We have {len(new_x)} elements in new wave")
set_interp = interpolate.interp1d(x, y, kind = 'linear')
new_y = set_interp(new_x)
new_y = new_y.astype(int)
# get last element of array and save it to new_y_last
new_y_last = new_y[len(new_y)-1]
# Plot resized wave
plt.plot(new_y)
plt.show()
# delete last element of array from our array
new_y = numpy.delete(new_y, -1)
# Edit YOUR_PATH to path you want to save your file:
# Example: C:\Users\Name\Desktop\my_file_2.csv
# Save array witout last element to csv file
numpy.savetxt(r'YOUR_PATH', new_y, newline = ',',fmt= '%-1.1d')
# Save last element to csv file (this remove last comma)
file = open(r'YOUR_PATH','a')
file.write(str(new_y_last))
file.close()