diff --git a/XP/preXP_970.bio b/XP/preXP_970.bio deleted file mode 100644 index 186ea50..0000000 Binary files a/XP/preXP_970.bio and /dev/null differ diff --git a/calibration_files/resisting_current_load_h1_b10.json b/calibration_files/resisting_current_load_h1_b10.json new file mode 100644 index 0000000..52e55a4 --- /dev/null +++ b/calibration_files/resisting_current_load_h1_b10.json @@ -0,0 +1,46 @@ +{ + "velocities": [ + -3.4122814094470955, + -6.824549608836408, + -10.234620627551287, + -13.646749013946947, + -17.05942871776043, + -20.47294103480792, + -23.884709681738556, + -27.29780505830115, + -30.712880698990137, + -34.12034771387118, + -37.53569106310214, + -40.941674348115434 + ], + "intensities": [ + 0.27811992627046433, + 0.388563680619895, + 0.45507799347401007, + 0.5016333329285085, + 0.5966743872537427, + 0.6681222593390906, + 0.790767386877535, + 1.013019271262357, + 1.0644758806845416, + 1.603867497285763, + 1.3974308929603743, + 2.079223339843453 + ], + "velocities_std": [ + 0.15779928107970184, + 0.23911234153327357, + 0.25111059899911664, + 0.1865149787760561, + 0.184596646626383, + 0.2098365288570435, + 0.26097255544231124, + 0.27930969495042535, + 0.2706046597893147, + 0.29650572466626124, + 0.3155743993285291, + 0.45500660602874476 + ], + "a": 0.1532295821802699, + "b": 2.211950144909677 +} \ No newline at end of file diff --git a/calibration_files/resisting_current_load_h2_b10.json b/calibration_files/resisting_current_load_h2_b10.json new file mode 100644 index 0000000..18a79ea --- /dev/null +++ b/calibration_files/resisting_current_load_h2_b10.json @@ -0,0 +1,46 @@ +{ + "velocities": [ + -3.4111452608211943, + -6.825293417432621, + -10.235405597564753, + -13.647771651953521, + -17.05809889568907, + -20.47276139032273, + -23.889240205287933, + -27.297842556174228, + -30.705112979657052, + -34.121111774840294, + -37.53102253614659, + -40.95016839513179 + ], + "intensities": [ + 0.40170171422509265, + 0.5083155407521315, + 0.7779980280844396, + 0.7614725392556392, + 0.6804280244965135, + 0.7338791749995764, + 0.7875363870536305, + 1.0023330315831753, + 1.073053893052075, + 1.5545523143599156, + 1.3764593297525105, + 2.0715453281924354 + ], + "velocities_std": [ + 0.1474620473416586, + 0.2167729543916627, + 0.2191314669079122, + 0.17031136339655675, + 0.17271796475737805, + 0.20051687826393055, + 0.23440110075564624, + 0.29359038632232787, + 0.276277034038277, + 0.31643478695779736, + 0.3076143770996864, + 0.4511988398237746 + ], + "a": 0.13262303350016438, + "b": 2.1496761211300517 +} \ No newline at end of file diff --git a/calibration_files/resisting_current_load_h3_b10.json b/calibration_files/resisting_current_load_h3_b10.json new file mode 100644 index 0000000..c2ba675 --- /dev/null +++ b/calibration_files/resisting_current_load_h3_b10.json @@ -0,0 +1,46 @@ +{ + "velocities": [ + -3.41018774289602, + -6.823433210298306, + -10.234794812519294, + -13.64791581906606, + -17.058045084923585, + -20.473977611860292, + -23.880214842856006, + -27.295889228313012, + -30.708536142268585, + -34.12319047718797, + -37.53119543541517, + -40.94779457177021 + ], + "intensities": [ + 0.2793413917745369, + 0.39268204670722306, + 0.3800897954528682, + 0.3995978906149249, + 0.48482392463512625, + 0.6180903599356785, + 0.7642747612350458, + 1.0129095765194842, + 1.0724695311894832, + 1.5526896493044486, + 1.4297274064381924, + 2.0822678325835455 + ], + "velocities_std": [ + 0.14831164873764094, + 0.2910761208852303, + 0.25337764837599736, + 0.17227498255130333, + 0.16684259827660206, + 0.19785991729136046, + 0.23763040620386328, + 0.27761032471639535, + 0.2726816812997153, + 0.298285186474567, + 0.29722746402149747, + 0.44066767628796616 + ], + "a": 0.1475475392584593, + "b": 2.10758354417158 +} \ No newline at end of file diff --git a/calibration_files/resisting_current_load_h4_b10.json b/calibration_files/resisting_current_load_h4_b10.json new file mode 100644 index 0000000..fe77c8a --- /dev/null +++ b/calibration_files/resisting_current_load_h4_b10.json @@ -0,0 +1,46 @@ +{ + "velocities": [ + -3.4102991025779503, + -6.823662862681027, + -10.235723641995877, + -13.649005398066432, + -17.061077590376122, + -20.475918812676852, + -23.884393839043476, + -27.299844849302097, + -30.71013031718303, + -34.1239041440288, + -37.53120730547922, + -40.95739669967116 + ], + "intensities": [ + 0.2788703943729869, + 0.36197982614478963, + 0.3501246342433477, + 0.37773427900047823, + 0.45729311672473827, + 0.6182559949787423, + 0.7711423931390106, + 1.0536233688430825, + 1.0747427176400006, + 1.6478858368781624, + 1.4527284180632292, + 2.174604301027509 + ], + "velocities_std": [ + 0.14245671888134204, + 0.27772532940937766, + 0.2561044394037885, + 0.18146919523564362, + 0.17002981438554657, + 0.1856615597223832, + 0.2329605819678024, + 0.27319433278391103, + 0.2611852419452288, + 0.30279233953456075, + 0.3061778355059749, + 0.4259669877063793 + ], + "a": 0.18172197236468093, + "b": 2.4441038535479733 +} \ No newline at end of file diff --git a/calibration_files/resisting_current_load_h5_b10.json b/calibration_files/resisting_current_load_h5_b10.json new file mode 100644 index 0000000..3bef699 --- /dev/null +++ b/calibration_files/resisting_current_load_h5_b10.json @@ -0,0 +1,46 @@ +{ + "velocities": [ + -3.4112367586011705, + -6.823847296121482, + -10.235175874183142, + -13.649853736407136, + -17.061385709458804, + -20.473021919436192, + -23.885489336940168, + -27.297403816845357, + -30.71061489292601, + -34.12809728038714, + -37.52393759806682, + -39.85482250659659 + ], + "intensities": [ + 0.30026767391172127, + 0.408398466131638, + 0.439372552884401, + 0.50103067072354, + 0.6214635672786373, + 0.7016135360705785, + 0.8222165507292368, + 1.034315063276005, + 1.1221861223527645, + 1.65475170272647, + 1.4504351004209117, + 2.157566047270202 + ], + "velocities_std": [ + 0.165461591936041, + 0.2793111478843145, + 0.2746668111608924, + 0.20132443176068454, + 0.18905529027564466, + 0.21422786028489924, + 0.24446205059791629, + 0.28758592488594525, + 0.2738639155716095, + 0.30086315074819003, + 0.3172034935871232, + 0.3451476644715911 + ], + "a": 0.20529709140653754, + "b": 2.5189724044568917 +} \ No newline at end of file diff --git a/calibration_files/resisting_current_load_hh_b1.json b/calibration_files/resisting_current_load_hh_b1.json new file mode 100644 index 0000000..9befe69 --- /dev/null +++ b/calibration_files/resisting_current_load_hh_b1.json @@ -0,0 +1,46 @@ +{ + "velocities": [ + -3.4120407462220586, + -6.825516923704728, + -10.238383447830685, + -13.64796277595554, + -17.064630128195486, + -20.476294325933317, + -23.88442265500885, + -27.30425639813553, + -30.715214906702542, + -34.1235301245941, + -37.53436877080029, + -40.94614917954288 + ], + "intensities": [ + 0.20134716162186858, + 0.1702650221351979, + 0.20586088855711746, + 0.26082112876762414, + 0.34875508223431334, + 0.519981824014429, + 0.5900712039616689, + 0.9144900400963688, + 1.009164603613374, + 1.518491570633324, + 1.3814229696799087, + 1.9581028206099105 + ], + "velocities_std": [ + 0.23798876502463326, + 0.1133226039405681, + 0.10196502625161918, + 0.11067510954684111, + 0.1207289251362517, + 0.13436317626303587, + 0.13875809498665231, + 0.16727527052278335, + 0.13259369373451754, + 0.1849650389184444, + 0.1643218331395464, + 0.40623457670842267 + ], + "a": 0.03380779858882961, + "b": 0.8872093238163651 +} \ No newline at end of file diff --git a/calibration_files/resisting_current_load_hh_b10.json b/calibration_files/resisting_current_load_hh_b10.json new file mode 100644 index 0000000..aa7a4eb --- /dev/null +++ b/calibration_files/resisting_current_load_hh_b10.json @@ -0,0 +1,49 @@ +{ + "velocities": [ + -3.4131318421539003, + -6.825291518567895, + -10.235453080804414, + -13.647720409250189, + -17.059243895968454, + -20.473035534966563, + -23.885975200013778, + -27.299204483423267, + -30.70959353775562, + -34.12389353978015, + -37.539720093584044, + -40.31614969667087, + -43.05072026543655 + ], + "intensities": [ + 0.3189242567582075, + 0.6439382942516751, + 0.6333561570994258, + 0.6723625093811607, + 0.6534935524053814, + 0.7224324734239292, + 0.8116124338671254, + 1.0532807705888074, + 1.1240853340444623, + 1.6428424185752302, + 1.4422977533628334, + 2.2712644428290885, + 2.564576979914713 + ], + "velocities_std": [ + 0.15890002927971816, + 0.2919853668120776, + 0.2588566578487991, + 0.19979624041730495, + 0.1888732226614335, + 0.20505021959167202, + 0.2419878419254406, + 0.2878692375460531, + 0.2923843259419282, + 0.3190076362138282, + 0.3273148411130423, + 0.38783138509707044, + 0.39911529950743857 + ], + "a": 0.19483370030561692, + "b": 2.517670315420316 +} \ No newline at end of file diff --git a/calibration_files/resisting_current_load_hh_b2.json b/calibration_files/resisting_current_load_hh_b2.json new file mode 100644 index 0000000..bfb172c --- /dev/null +++ b/calibration_files/resisting_current_load_hh_b2.json @@ -0,0 +1,46 @@ +{ + "velocities": [ + -3.4126778307158743, + -6.823725593082668, + -10.23750268395414, + -13.650760427263458, + -17.06491538017027, + -20.477396532146432, + -23.88639135840322, + -27.303663151284002, + -30.707168174368594, + -34.13344826309183, + -37.544105193827626, + -40.942522948940855 + ], + "intensities": [ + 0.20018342983745457, + 0.17955567669673272, + 0.2134997427687467, + 0.2655619622139137, + 0.3476182566914793, + 0.5210104978328366, + 0.5617469793859872, + 0.8717163503416356, + 1.036202590148378, + 1.560986389624314, + 1.3818801642761775, + 1.9686330620998214 + ], + "velocities_std": [ + 0.22083991379312792, + 0.10508787921939915, + 0.10989697305512404, + 0.11519432771257837, + 0.13071221460865895, + 0.14486091445928367, + 0.13364660430486577, + 0.19317633038210588, + 0.1534370840317438, + 0.17940358090886516, + 0.18660607191495165, + 0.3513860430631813 + ], + "a": 0.04115635809031276, + "b": 0.8785364704740983 +} \ No newline at end of file diff --git a/calibration_files/resisting_current_load_hh_b3.json b/calibration_files/resisting_current_load_hh_b3.json new file mode 100644 index 0000000..dd17897 --- /dev/null +++ b/calibration_files/resisting_current_load_hh_b3.json @@ -0,0 +1,46 @@ +{ + "velocities": [ + -3.4123896628864365, + -6.823475531269763, + -10.235010744590234, + -13.648227169566894, + -17.061631189090633, + -20.47760564930501, + -23.886034983079075, + -27.30167534775249, + -30.70592108100009, + -34.12458760693233, + -37.539284611921, + -40.94120913344914 + ], + "intensities": [ + 0.18504869366732601, + 0.18040004561274464, + 0.21048279980126627, + 0.2716968499998153, + 0.3574053716576325, + 0.527519439307864, + 0.6197824425410143, + 0.9201933799278003, + 1.0457696392739992, + 1.6170590382037173, + 1.3789959725356447, + 2.009044110931159 + ], + "velocities_std": [ + 0.1627553117521026, + 0.10472010730253321, + 0.1120183647242364, + 0.12617343248040824, + 0.14355871092305986, + 0.15036449578869204, + 0.15631255571472757, + 0.19754824337495822, + 0.17948322897881377, + 0.19088344955464104, + 0.20264665390783718, + 0.38415466304259477 + ], + "a": 0.04892395040994347, + "b": 1.0157003238159605 +} \ No newline at end of file diff --git a/calibration_files/resisting_current_load_hh_b4.json b/calibration_files/resisting_current_load_hh_b4.json new file mode 100644 index 0000000..c71d994 --- /dev/null +++ b/calibration_files/resisting_current_load_hh_b4.json @@ -0,0 +1,46 @@ +{ + "velocities": [ + -3.4121695082027976, + -6.824554932625013, + -10.237850779499658, + -13.6509354846196, + -17.06336093161059, + -20.471121755769413, + -23.88501364798773, + -27.30433403749416, + -30.7207879063268, + -34.121877469140095, + -37.53141209960297, + -40.9489961419481 + ], + "intensities": [ + 0.18311243328323315, + 0.187533658879728, + 0.21236315566535074, + 0.28105213186169564, + 0.3638344459012182, + 0.5335937085713144, + 0.6389214948229889, + 0.9284447624411939, + 1.0432386334844175, + 1.560147466793445, + 1.4120693849335735, + 2.075535626001884 + ], + "velocities_std": [ + 0.14489564803913885, + 0.12160559736002967, + 0.11792007625558881, + 0.1391748512997074, + 0.1437118621134034, + 0.15086001839185512, + 0.18820911950823035, + 0.21776715638227392, + 0.2049390704876942, + 0.227025297906265, + 0.2073145073516913, + 0.36962054875896033 + ], + "a": 0.05651973552639991, + "b": 1.0875613214864253 +} \ No newline at end of file diff --git a/calibration_files/resisting_current_load_hh_b6.json b/calibration_files/resisting_current_load_hh_b6.json new file mode 100644 index 0000000..a70c193 --- /dev/null +++ b/calibration_files/resisting_current_load_hh_b6.json @@ -0,0 +1,46 @@ +{ + "velocities": [ + -3.4124769054438486, + -6.825273832449391, + -10.237663557592567, + -13.650054780278111, + -17.061859206410613, + -20.474653689169614, + -23.887637180609374, + -27.30001380348773, + -30.713970353620937, + -34.12433227930313, + -37.53718291705637, + -40.94872058013526 + ], + "intensities": [ + 0.20722168177758726, + 0.20328868232157235, + 0.2384488114258002, + 0.3013598439045397, + 0.39776098987596525, + 0.542224024924887, + 0.663479777329414, + 0.9671075741813547, + 1.0678230460936728, + 1.6448125676914274, + 1.4322365713063254, + 2.094336799602215 + ], + "velocities_std": [ + 0.13692546070039555, + 0.12679425223290602, + 0.1476552712374031, + 0.15523431730900583, + 0.18242578102937793, + 0.19299044985143615, + 0.17760162727004347, + 0.1931767949559949, + 0.1707174088035144, + 0.1876724911736178, + 0.1987595743580701, + 0.33865624253706494 + ], + "a": 0.0851339004401859, + "b": 1.3342928731568007 +} \ No newline at end of file diff --git a/calibration_files/resisting_current_load_hh_b8.json b/calibration_files/resisting_current_load_hh_b8.json new file mode 100644 index 0000000..3be3a75 --- /dev/null +++ b/calibration_files/resisting_current_load_hh_b8.json @@ -0,0 +1,46 @@ +{ + "velocities": [ + -3.4122947257249763, + -6.825006860481938, + -10.238273648260948, + -13.651301161413352, + -17.063185445069447, + -20.475610864104105, + -23.8888449746979, + -27.300071102317013, + -30.71372027157043, + -34.12789496733372, + -37.54037633430527, + -40.955089894315165 + ], + "intensities": [ + 0.2208736286540598, + 0.22807247964490554, + 0.30704565826013497, + 0.3211250403209993, + 0.39397329761149125, + 0.5693886055632087, + 0.6937546112360465, + 0.9903805238413066, + 1.0729209878044244, + 1.562140967389249, + 1.3685302540860678, + 2.0224120512064334 + ], + "velocities_std": [ + 0.13571116224010635, + 0.14283492009058515, + 0.21004419096417162, + 0.19164749313483445, + 0.1559303600236407, + 0.14901243153649268, + 0.1689725634084108, + 0.2060416304565691, + 0.19381139269424097, + 0.22386149754375761, + 0.22413923670366878, + 0.3640467352272724 + ], + "a": 0.1223547649293402, + "b": 1.6190083488141043 +} \ No newline at end of file diff --git a/calibration_files/resisting_current_load_hl_b10.json b/calibration_files/resisting_current_load_hl_b10.json new file mode 100644 index 0000000..d8ce954 --- /dev/null +++ b/calibration_files/resisting_current_load_hl_b10.json @@ -0,0 +1,46 @@ +{ + "velocities": [ + -3.409737442179061, + -6.823610077178679, + -10.234296717346671, + -13.64679238220741, + -17.059681444479814, + -20.470556000633948, + -23.88306303353799, + -27.29311261272856, + -30.70529648460968, + -34.11653378821919, + -37.52937806346429, + -40.940233886266455 + ], + "intensities": [ + 0.295462438367951, + 0.3489281184096797, + 0.3443942732068211, + 0.35699830471635124, + 0.42093845958635534, + 0.5775719373560323, + 0.7297574205610777, + 0.9339640778047322, + 1.043738798065894, + 1.5122314393157943, + 1.38686032572716, + 2.013714849584207 + ], + "velocities_std": [ + 0.17045843404589545, + 0.2562122376281941, + 0.23419495371700577, + 0.15068923269466547, + 0.15185582579687895, + 0.1923588947242439, + 0.25801002799615497, + 0.25066234787152547, + 0.2514374076224203, + 0.28587503682487897, + 0.31268509674467615, + 0.4406270796421223 + ], + "a": 0.06913469565407133, + "b": 2.035780172578649 +} \ No newline at end of file diff --git a/computation.py b/computation.py new file mode 100644 index 0000000..108ff28 --- /dev/null +++ b/computation.py @@ -0,0 +1,51 @@ +import json +import matplotlib.pyplot as plt +import numpy as np +from scipy.optimize import curve_fit + +a = [] +b = [] +h = "h" +for bike in ["10", "8", "6", "4", "3", "2", "1"]: + with open(f"./calibration_files/resisting_current_load_h{h}_b{bike}.json", "r") as f: + data = json.load(f) + a.append(data["a"]) + b.append(data["b"]) + + +def find(pig, prop, cst): + return prop / pig + cst + + +pig = np.asarray([11, 15, 19, 25, 28, 32, 36]) + +popt = curve_fit(find, pig, a) + + +plt.plot(pig, a) +plt.plot(pig, find(pig, *popt[0])) +print(popt[0]) +plt.show() + +a = [] +b = [] +bike = "10" +for h in ["h", "5", "4", "3", "2", "1", "l"]: + with open(f"./calibration_files/resisting_current_load_h{h}_b{bike}.json", "r") as f: + data = json.load(f) + a.append(data["a"]) + b.append(data["b"]) + + +def find(ht, prop, cst): + return prop * ht + cst + + +ht = np.asarray([6, 5, 4, 3, 2, 1, 0]) + +popt = curve_fit(find, ht, a) +print(popt[0]) + +plt.plot(ht, a) +plt.plot(ht, find(ht, *popt[0])) +plt.show() diff --git a/computation_2.py b/computation_2.py new file mode 100644 index 0000000..5b8e0e3 --- /dev/null +++ b/computation_2.py @@ -0,0 +1,68 @@ +import json +import matplotlib.pyplot as plt +import numpy as np + +from scipy.optimize import curve_fit + +resisting_current_proportional = [] +resisting_current_constant = [] +x = np.zeros((2, 12)) +h = { + "h": 6, + "5": 5, + "4": 4, + "3": 3, + "2": 2, + # "1": 1, + "l": 0, +} +bike = {"10": 11, "8": 15, "6": 19, "4": 25, "3": 28, "2": 32, "1": 36} +for ht in h.keys(): + if ht == "h": + for bk in bike.keys(): + with open(f"./calibration_files/resisting_current_load_h{ht}_b{bk}.json", "r") as f: + data = json.load(f) + resisting_current_proportional.append(data["a"]) + resisting_current_constant.append(data["b"]) + x[0, len(resisting_current_proportional) - 1] = h[ht] + x[1, len(resisting_current_proportional) - 1] = bike[bk] + + else: + with open(f"./calibration_files/resisting_current_load_h{ht}_b10.json", "r") as f: + data = json.load(f) + resisting_current_proportional.append(data["a"]) + resisting_current_constant.append(data["b"]) + x[0, len(resisting_current_proportional) - 1] = h[ht] + x[1, len(resisting_current_proportional) - 1] = bike["10"] + + +def find_pig(x, coeff_pig, coeff_ht, coeff_cst): + return coeff_pig / x[1] + coeff_ht * x[0] + coeff_cst + + +popt_constant = curve_fit(find_pig, x, resisting_current_constant, p0=[-2.53, -0.023, 0.0]) +popt_proportional = curve_fit(find_pig, x, resisting_current_proportional, p0=[-2.53, -0.023, 0.0]) +print(popt_constant[0]) + +plt.plot(x[0, :], resisting_current_constant, "o") +plt.plot(x[0, :], find_pig(x, *popt_constant[0]), "o") +plt.show() + +plt.plot(x[1, :], resisting_current_constant, "o") +plt.plot(x[1, :], find_pig(x, *popt_constant[0]), "o") +plt.show() + +with open("./ergocycleS2M/parameters/hardware_and_security.json", "r") as f: + hardware_and_security = json.load(f) + +hardware_and_security["resisting_current_cst_gear"] = popt_constant[0][0] +hardware_and_security["resisting_current_cst_hometrainer"] = popt_constant[0][1] +hardware_and_security["resisting_current_cst_constant"] = popt_constant[0][2] +hardware_and_security["resisting_current_prop_gear"] = popt_proportional[0][0] +hardware_and_security["resisting_current_prop_hometrainer"] = popt_proportional[0][1] +hardware_and_security["resisting_current_prop_constant"] = popt_proportional[0][2] + +# Writing to .json +json_object = json.dumps(hardware_and_security, indent=4) +with open("./ergocycleS2M/parameters/hardware_and_security.json", "w") as outfile: + outfile.write(json_object) diff --git a/ergocycleS2M/application/application.py b/ergocycleS2M/application/application.py index c84f577..d4546d2 100644 --- a/ergocycleS2M/application/application.py +++ b/ergocycleS2M/application/application.py @@ -7,7 +7,7 @@ import sys import time -from ctypes import c_bool, c_double, c_long, c_wchar_p +from ctypes import c_bool, c_double, c_int, c_long, c_wchar_p from PyQt5 import QtWidgets from ergocycleS2M.data_processing.save import DataSaver @@ -17,6 +17,9 @@ from ergocycleS2M.motor_control.motor_controller import MotorController # from ergocycleS2M.motor_control.mock_controller import MockController +# Import the feedback window (assuming you saved it in a module) +from ergocycleS2M.gui.gui2 import AverageCadenceFeedbackWindow + class Application: """ @@ -101,6 +104,9 @@ def __init__(self, save_period: float = 10): # Shared memory # Security self.run = mp.Manager().Value(c_bool, True) + # Hardware + self.gear = mp.Manager().Value(c_int, 0) + self.hometrainer = mp.Manager().Value(c_int, 0) # Control self.zero_position = mp.Manager().Value(c_bool, 0.0) self.queue_instructions = mp.Manager().Queue() @@ -132,6 +138,8 @@ def __init__(self, save_period: float = 10): self.motor_error = mp.Manager().Value(c_long, 0) self.sensorless_estimator_error = mp.Manager().Value(c_long, 0) self.can_error = mp.Manager().Value(c_long, 0) + #self.update_period = 0.1, + #self.plot_window_size = 5, # Create the processes self.gui_process = mp.Process(name="GUI process", target=self.gui, daemon=True) @@ -157,6 +165,8 @@ def motor_control_loop(self): motor = MotorController(enable_watchdog=True, external_watchdog=False) motor.zero_position_calibration() is_cadence_control = False + current_mode = None + current_target = None while self.run.value: control_mode = motor.get_control_mode() @@ -166,10 +176,18 @@ def motor_control_loop(self): self.zero_position.value = False if not self.stopping.value: - # Adapt the control of the motor accordingly to the current cadence and torque try: - control_mode, direction = self.queue_instructions.get_nowait() + instruction = self.queue_instructions.get_nowait() + if len(instruction) == 2: + control_mode, direction = instruction + target_angle = None + elif len(instruction) == 3: + control_mode, direction, target_angle = instruction + else: + continue motor.set_direction(direction) + current_mode = control_mode + current_target = target_angle is_cadence_control = False except Exception: pass @@ -178,19 +196,23 @@ def motor_control_loop(self): # because of the resisting torque. # Furthermore, it allows to stop the pedals by reducing the torque if the user has stopped. if control_mode == ControlMode.TORQUE_CONTROL: - self.instruction.value = motor.torque_control(self.spin_box.value, self.ramp_instruction.value) + self.instruction.value = motor.torque_control( + self.spin_box.value, self.ramp_instruction.value, self.gear.value, self.hometrainer.value + ) # The concentric power control mode is based on the torque control mode, but the torque input is # calculated from the current cadence (torque_input = f(power / cadence, resiting torque)). elif control_mode == ControlMode.CONCENTRIC_POWER_CONTROL: self.instruction.value = motor.concentric_power_control( - self.spin_box.value, self.ramp_instruction.value + self.spin_box.value, self.ramp_instruction.value, self.gear.value, self.hometrainer.value ) # The linear control mode is based on the torque control mode, but the torque input is calculated from # the current cadence (torque_input = f(linear_coeff * cadence, resiting torque)). elif control_mode == ControlMode.LINEAR_CONTROL: - self.instruction.value = motor.linear_control(self.spin_box.value, self.ramp_instruction.value) + self.instruction.value = motor.linear_control( + self.spin_box.value, self.ramp_instruction.value, self.gear.value, self.hometrainer.value + ) # The concentric power control mode is based on the cadence control mode, but the cadence input is # calculated from the current torque (cadence_input = f(power / torque, resiting torque)). @@ -205,6 +227,10 @@ def motor_control_loop(self): motor.cadence_control(self.spin_box.value, self.ramp_instruction.value) is_cadence_control = True + elif current_mode == ControlMode.ISOMETRIC_CONTROL and current_target is not None: + motor.isometric_control(current_target, self.ramp_instruction.value) + current_mode = ControlMode.STOP + else: motor.stopping(cadence_ramp_rate=self.ramp_instruction.value) if abs(motor.get_cadence()) < 10.0: @@ -227,6 +253,7 @@ def motor_control_loop(self): self.can_error.value = motor.get_can_error() self.motor_time.value = time.time() + self.user_torque.value = motor.get_user_torque() def gui(self): """ @@ -234,8 +261,11 @@ def gui(self): update the control of the motor in the shared memory. """ app = QtWidgets.QApplication(sys.argv) + # Create the main Ergocycle GUI window. gui = ErgocycleGUI( run=self.run, + gear=self.gear, + hometrainer=self.hometrainer, zero_position=self.zero_position, queue_instructions=self.queue_instructions, training_mode=self.training_mode, @@ -258,8 +288,20 @@ def gui(self): motor_error=self.motor_error, sensorless_estimator_error=self.sensorless_estimator_error, can_error=self.can_error, + user_torque=self.user_torque, + #update_period=self.plot_window_size, + #plot_window_size=self.plot_window_size, ) + + # Create the Average Cadence Feedback Window. + feedback_window = AverageCadenceFeedbackWindow() + + # Attach the feedback window instance to the main GUI. + gui.feedback_window = feedback_window + + # Show both windows. gui.show() + feedback_window.show() app.exec() def data_saver(self): @@ -279,10 +321,14 @@ def data_saver(self): i += 1 file_name = f"{file_name}_{i}" + print("Saving") + saving_widget = DataSaver( file_path=file_name, run=self.run, saving=self.saving, + gear=self.gear, + hometrainer=self.hometrainer, spin_box=self.spin_box, instruction=self.instruction, ramp_instruction=self.ramp_instruction, @@ -303,10 +349,11 @@ def data_saver(self): motor_error=self.motor_error, sensorless_estimator_error=self.sensorless_estimator_error, can_error=self.can_error, + user_torque=self.user_torque, save_period=self.save_period, motor_time=self.motor_time, ) saving_app.exec() except Exception: # No file name in the queue meaning that no saving instruction has been sent. - pass + pass \ No newline at end of file diff --git a/ergocycleS2M/data_processing/load.py b/ergocycleS2M/data_processing/load.py index 4013ca8..bf25ea4 100644 --- a/ergocycleS2M/data_processing/load.py +++ b/ergocycleS2M/data_processing/load.py @@ -74,6 +74,8 @@ def compute_data( vel_estimate: np.ndarray, turns: np.ndarray, iq_measured: np.ndarray, + gear: np.ndarray, + hometrainer: np.ndarray, ) -> Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray]: """ Computes the data of interest from raw data saved. @@ -104,9 +106,11 @@ def compute_data( for i in range(nb_points): cadence[i] = motor_object.compute_cadence(vel_estimate[i]) angle[i] = motor_object.compute_angle(turns[i]) - user_torque[i] = motor_object.compute_user_torque(iq_measured[i], vel_estimate[i]) + user_torque[i] = motor_object.compute_user_torque(iq_measured[i], vel_estimate[i], gear[i], hometrainer[i]) user_power[i] = motor_object.compute_user_power(user_torque[i], cadence[i]) - resisting_torque[i] = motor_object.compute_resisting_torque(iq_measured[i], vel_estimate[i]) + resisting_torque[i] = motor_object.compute_resisting_torque( + iq_measured[i], vel_estimate[i], gear[i], hometrainer[i] + ) motor_torque[i] = motor_object.compute_motor_torque(iq_measured[i]) return user_torque, cadence, angle, user_power, resisting_torque, motor_torque @@ -151,7 +155,13 @@ def interpolate_data(data: dict, frequency: float = 10) -> dict: data_interpolated["lap"][j] = 0.0 # For the other keys, which are strings, instructions or errors, we take the nearest value. + if "gear" not in data.keys(): + data["gear"] = np.zeros(len(data["time"])) + if "hometrainer" not in data.keys(): + data["hometrainer"] = np.zeros(len(data["time"])) nearest_keys = [ + "gear", + "hometrainer", "state", "control_mode", "direction", @@ -268,7 +278,13 @@ def read(data_path: str, sample_frequency: float, window_length: int) -> dict: data_interpolated["user_power"], data_interpolated["resisting_torque"], data_interpolated["motor_torque"], - ) = compute_data(data_interpolated["vel_estimate"], data_interpolated["turns"], data_interpolated["iq_measured"]) + ) = compute_data( + data_interpolated["vel_estimate"], + data_interpolated["turns"], + data_interpolated["iq_measured"], + data_interpolated["gear"], + data_interpolated["hometrainer"], + ) smoothed_data = smooth_data(data_interpolated, window_length) return smoothed_data diff --git a/ergocycleS2M/data_processing/save.py b/ergocycleS2M/data_processing/save.py index a550492..f776c0f 100644 --- a/ergocycleS2M/data_processing/save.py +++ b/ergocycleS2M/data_processing/save.py @@ -34,6 +34,8 @@ def save(data_dict, data_path): def save_data_to_file( file_path: str, time: float = None, + gear: int = 0, + hometrainer: int = 0, spin_box: float = None, instruction: float = None, ramp_instruction: float = None, @@ -111,6 +113,8 @@ def save_data_to_file( data = { "time": time, "spin_box": spin_box, + "gear": gear, + "hometrainer": hometrainer, "instruction": instruction, "ramp_instruction": ramp_instruction, "comments": comment, @@ -141,6 +145,8 @@ def __init__( file_path: str, run: mp.Value, saving: mp.Value, + gear: mp.Value, + hometrainer: mp.Value, spin_box: mp.Value, instruction: mp.Value, ramp_instruction: mp.Value, @@ -161,8 +167,9 @@ def __init__( motor_error: mp.Value, sensorless_estimator_error: mp.Value, can_error: mp.Value, + user_torque, motor_time: mp.Value, - save_period: float = 0.1, + save_period: float = 0.01, ): super().__init__(parent=None) self.save_period = save_period @@ -172,6 +179,8 @@ def __init__( # Data self.file_path = file_path + self.gear = gear + self.hometrainer = hometrainer self.spin_box = spin_box self.instruction = instruction self.ramp_instruction = ramp_instruction @@ -193,6 +202,7 @@ def __init__( self.motor_error = motor_error self.sensorless_estimator_error = sensorless_estimator_error self.can_error = can_error + self.user_torque = user_torque self.motor_time = motor_time @@ -229,6 +239,8 @@ def save_data_to_file(self): data = { "time": time.time() - self.start_time, "motor_time": self.motor_time.value, + "gear": self.gear.value, + "hometrainer": self.hometrainer.value, "spin_box": self.spin_box.value, "instruction": self.instruction.value, "ramp_instruction": self.ramp_instruction.value, @@ -249,6 +261,8 @@ def save_data_to_file(self): "motor_error": self.motor_error.value, "sensorless_estimator_error": self.sensorless_estimator_error.value, "can_error": self.can_error.value, + "user_torque": self.user_torque.value, + } save(data, self.file_path) diff --git a/ergocycleS2M/gui/enums.py b/ergocycleS2M/gui/enums.py index 3a8344d..e103dbd 100644 --- a/ergocycleS2M/gui/enums.py +++ b/ergocycleS2M/gui/enums.py @@ -24,6 +24,7 @@ class GUIControlMode(StrEnum): CADENCE = "Cadence" TORQUE = "Torque" POWER = "Power" + ISOMETRIC = "Isometric" class StopwatchStates(Enum): diff --git a/ergocycleS2M/gui/ergocycle_gui.py b/ergocycleS2M/gui/ergocycle_gui.py index 9550984..de064ea 100644 --- a/ergocycleS2M/gui/ergocycle_gui.py +++ b/ergocycleS2M/gui/ergocycle_gui.py @@ -14,7 +14,7 @@ class Ui_MainWindow(object): def setupUi(self, MainWindow): MainWindow.setObjectName("MainWindow") - MainWindow.resize(1060, 714) + MainWindow.resize(1060, 769) MainWindow.setMinimumSize(QtCore.QSize(0, 0)) MainWindow.setMaximumSize(QtCore.QSize(1060, 16777215)) self.centralwidget = QtWidgets.QWidget(MainWindow) @@ -150,6 +150,36 @@ def setupUi(self, MainWindow): self.command_verticalLayout.addWidget(self.line_4) self.command_horizontalLayout = QtWidgets.QHBoxLayout() self.command_horizontalLayout.setObjectName("command_horizontalLayout") + self.verticalLayout = QtWidgets.QVBoxLayout() + self.verticalLayout.setObjectName("verticalLayout") + self.gear_label = QtWidgets.QLabel(self.centralwidget) + font = QtGui.QFont() + font.setPointSize(8) + self.gear_label.setFont(font) + self.gear_label.setObjectName("gear_label") + self.verticalLayout.addWidget(self.gear_label) + self.gear_spinBox = QtWidgets.QSpinBox(self.centralwidget) + self.gear_spinBox.setMaximumSize(QtCore.QSize(50, 16777215)) + self.gear_spinBox.setObjectName("gear_spinBox") + self.verticalLayout.addWidget(self.gear_spinBox) + self.hometrainer_label = QtWidgets.QLabel(self.centralwidget) + font = QtGui.QFont() + font.setPointSize(8) + self.hometrainer_label.setFont(font) + self.hometrainer_label.setObjectName("hometrainer_label") + self.verticalLayout.addWidget(self.hometrainer_label) + self.hometrainer_spinBox = QtWidgets.QSpinBox(self.centralwidget) + self.hometrainer_spinBox.setMaximumSize(QtCore.QSize(50, 16777215)) + self.hometrainer_spinBox.setObjectName("hometrainer_spinBox") + self.verticalLayout.addWidget(self.hometrainer_spinBox) + self.command_horizontalLayout.addLayout(self.verticalLayout) + self.line_5 = QtWidgets.QFrame(self.centralwidget) + self.line_5.setLineWidth(0) + self.line_5.setMidLineWidth(20) + self.line_5.setFrameShape(QtWidgets.QFrame.VLine) + self.line_5.setFrameShadow(QtWidgets.QFrame.Sunken) + self.line_5.setObjectName("line_5") + self.command_horizontalLayout.addWidget(self.line_5) self.verticalLayout_8 = QtWidgets.QVBoxLayout() self.verticalLayout_8.setObjectName("verticalLayout_8") self.label = QtWidgets.QLabel(self.centralwidget) @@ -434,6 +464,8 @@ def retranslateUi(self, MainWindow): self.average_power_label.setText(_translate("MainWindow", "Average power")) self.average_power_display.setText(_translate("MainWindow", "TextLabel")) self.emergency_pushButton.setText(_translate("MainWindow", "EMERGENCY STOP")) + self.gear_label.setText(_translate("MainWindow", "Gear")) + self.hometrainer_label.setText(_translate("MainWindow", "Home trainer")) self.label.setText(_translate("MainWindow", "Reset the angle and turns")) self.angle_reset_pushButton.setText(_translate("MainWindow", "0 ° and 0 tr")) self.stop_pushButton.setText(_translate("MainWindow", "Stop")) diff --git a/ergocycleS2M/gui/ergocycle_gui.ui b/ergocycleS2M/gui/ergocycle_gui.ui index df618e5..57107c9 100644 --- a/ergocycleS2M/gui/ergocycle_gui.ui +++ b/ergocycleS2M/gui/ergocycle_gui.ui @@ -7,7 +7,7 @@ 0 0 1060 - 714 + 769 @@ -344,6 +344,67 @@ + + + + + + + 8 + + + + Gear + + + + + + + + 50 + 16777215 + + + + + + + + + 8 + + + + Home trainer + + + + + + + + 50 + 16777215 + + + + + + + + + + 0 + + + 20 + + + Qt::Vertical + + + diff --git a/ergocycleS2M/gui/gui.py b/ergocycleS2M/gui/gui.py index 6d2b050..d60c9f9 100644 --- a/ergocycleS2M/gui/gui.py +++ b/ergocycleS2M/gui/gui.py @@ -23,11 +23,15 @@ traduce_error, ) +from ergocycleS2M.gui.gui2 import AverageCadenceFeedbackWindow + class ErgocycleGUI(QtWidgets.QMainWindow): def __init__( self, + gear: mp.Manager().Value, + hometrainer: mp.Manager().Value, run: mp.Manager().Value, zero_position: mp.Manager().Value, queue_instructions: mp.Queue, @@ -51,8 +55,9 @@ def __init__( motor_error: mp.Manager().Value, sensorless_estimator_error: mp.Manager().Value, can_error: mp.Manager().Value, + user_torque: mp.Manager().Value, update_period: float = 0.1, - plot_window_size: int = 10, + plot_window_size: int = 5, ): super(ErgocycleGUI, self).__init__(parent=None) self.ui = Ui_MainWindow() @@ -64,6 +69,9 @@ def __init__( # Shared memory # Security self.run = run + # Hardware + self.gear = gear + self.hometrainer = hometrainer # Control self.zero_position = zero_position self.queue_instruction = queue_instructions @@ -83,6 +91,7 @@ def __init__( self.i_measured = i_measured self.turns = turns self.vel_estimate = vel_estimate + self.user_torque = user_torque # Errors self.error = error self.axis_error = axis_error @@ -104,6 +113,19 @@ def __init__( ) self._color_grey = QtGui.QColor(220, 220, 220) + # Hardware + self.gui_gear = 0 + self.ui.gear_spinBox.valueChanged.connect(self._update_gear) + self.ui.gear_spinBox.setValue(0) + self.ui.gear_spinBox.setSingleStep(1) + self.ui.gear_spinBox.setRange(0, 10) + self.gui_hometrainer = 0 + self.ui.hometrainer_spinBox.valueChanged.connect(self._update_hometrainer) + self.ui.hometrainer_spinBox.setValue(0) + self.ui.hometrainer_spinBox.setSingleStep(1) + self.ui.hometrainer_spinBox.setRange(0, 6) + self.ui.hometrainer_spinBox.setEnabled(False) + # Control self.motor_started = False self._gui_control_mode = GUIControlMode.POWER @@ -161,6 +183,7 @@ def __init__( self.ui.torque_horizontalLayout.insertWidget(0, self.plot_torque) self.plot_window_size = plot_window_size self.size_arrays = int(self.plot_window_size * 1 / update_period) # 20 seconds + self.size_1s = int(1 / update_period) self.time_array = np.linspace(-self.plot_window_size, 0, self.size_arrays) self.cadence_array = np.zeros(self.size_arrays) self.torque_array = np.zeros(self.size_arrays) @@ -208,6 +231,13 @@ def _check_text(self, text): self.ui.save_lineEdit.setText(new_text) self.ui.save_lineEdit.setCursorPosition(cursor_pos - 1) + def _update_gear(self): + self.gear.value = self.gui_gear = self.ui.gear_spinBox.value() + self.ui.hometrainer_spinBox.setEnabled(self.gear.value != 0) + + def _update_hometrainer(self): + self.hometrainer.value = self.gui_hometrainer = self.ui.hometrainer_spinBox.value() + def _update_instruction_display_on_training_mode_change(self): """ Update the display accordingly to the training mode. @@ -226,6 +256,7 @@ def _update_instruction_display_on_training_mode_change(self): GUIControlMode.CADENCE.value, GUIControlMode.LINEAR.value, GUIControlMode.TORQUE.value, + GUIControlMode.ISOMETRIC.value ] ) self.ui.direction_comboBox.setCurrentText(DirectionMode.FORWARD.value) @@ -246,9 +277,9 @@ def _update_instruction_display_on_control_mode_change(self): Update the display accordingly to the control mode. """ self._set_instruction_to_0() - power_step = 10 + power_step = 5 vel_step = 10 - vel_ramp = 30 + vel_ramp = 10 torque_step = 1 torque_ramp = 5 linear_step = 0.1 @@ -322,6 +353,18 @@ def _update_instruction_display_on_control_mode_change(self): self.ui.units_label.setText("N.m") self.ui.ramp_label.setText("Torque ramp") self.ui.acceleration_units_label.setText("N.m/s") + + elif control_mode == GUIControlMode.ISOMETRIC.value: + self.ui.instruction_spinBox.setRange(0, 360) + self.ui.instruction_spinBox.setSingleStep(10) + self.ui.instruction_spinBox.setValue(90) + self.ui.instruction_label.setText("Angle cible") + self.ui.units_label.setText("°") + self.ui.acceleration_spinBox.setRange(0, self.motor_computations.hardware_and_security["pedals_accel_lim"] - 1) + self.ui.acceleration_spinBox.setSingleStep(vel_step) + self.ui.acceleration_spinBox.setValue(vel_ramp) + self.ui.ramp_label.setText("Rampe de cadence") + self.ui.acceleration_units_label.setText("rpm/s") else: raise NotImplementedError(f"{control_mode} control has not been implemented yet.") @@ -398,6 +441,15 @@ def _control_update(self): self._get_sign(self.ui.direction_comboBox.currentText()) * self.ui.instruction_spinBox.value() ) self.queue_instruction.put_nowait((ControlMode.TORQUE_CONTROL, self.ui.direction_comboBox.currentText())) + + elif self._gui_control_mode == GUIControlMode.ISOMETRIC: + target_angle = self.ui.instruction_spinBox.value() + self.spin_box.value = target_angle + self.queue_instruction.put_nowait(( + ControlMode.ISOMETRIC_CONTROL, + self.ui.direction_comboBox.currentText(), + target_angle + )) else: raise NotImplementedError(f"{self._gui_control_mode} control has not been implemented yet.") @@ -615,7 +667,8 @@ def loop(self): turns = self.turns.value vel_estimate = self.vel_estimate.value - user_torque = self.motor_computations.compute_user_torque(i_measured, vel_estimate) + user_torque = self.motor_computations.compute_user_torque(i_measured, vel_estimate, self.gui_gear, self.gui_hometrainer) + self.user_torque.value = user_torque cadence = self.motor_computations.compute_cadence(vel_estimate) user_power = self.motor_computations.compute_user_power(user_torque, cadence) angle = self.motor_computations.compute_angle(turns) @@ -659,3 +712,47 @@ def loop(self): else: self.spin_box_array = None self._plot_update() + + + average_cadence = float(np.mean(self.cadence_array[-self.size_1s:])) + + + # Tell the feedback window about the new average + self.feedback_window.update_value(average_cadence) + + # Then force its bar-plot redraw + self.feedback_window.update_plot() + + + def keyPressEvent(self, event): + # Only modify the power target if we're in POWER mode. + if self._gui_control_mode == GUIControlMode.POWER.value: + # Get the step from the spin box settings (or set it explicitly) + + + power_step = self.ui.instruction_spinBox.singleStep() + + if event.key() == QtCore.Qt.Key_A: + # Increase power target + new_value = self.ui.instruction_spinBox.value() + power_step + # Clamp to the maximum allowed value + new_value = min(new_value, self.ui.instruction_spinBox.maximum()) + self.ui.instruction_spinBox.setValue(new_value) + # Simulate pressing the update button by calling _control_update + self._control_update() + event.accept() + return + + elif event.key() == QtCore.Qt.Key_B: + # Decrease power target + new_value = self.ui.instruction_spinBox.value() - power_step + # Clamp to the minimum allowed value + new_value = max(new_value, self.ui.instruction_spinBox.minimum()) + self.ui.instruction_spinBox.setValue(new_value) + # Simulate pressing the update button by calling _control_update + self._control_update() + event.accept() + return + + # For all other keys, call the base implementation. + super(ErgocycleGUI, self).keyPressEvent(event) diff --git a/ergocycleS2M/gui/gui2.py b/ergocycleS2M/gui/gui2.py new file mode 100644 index 0000000..a85a521 --- /dev/null +++ b/ergocycleS2M/gui/gui2.py @@ -0,0 +1,120 @@ +import sys +import time +import numpy as np +import matplotlib.pyplot as plt + +from PyQt5 import QtWidgets, QtCore +from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas + + +class AverageCadenceFeedbackWindow(QtWidgets.QMainWindow): + def __init__(self, parent=None): + super(AverageCadenceFeedbackWindow, self).__init__(parent) + self.setWindowTitle("Average Cadence Feedback") + self.setWindowFlags(self.windowFlags() | QtCore.Qt.Window) + self.resize(1000, 1000) + + # Central widget and layout + central_widget = QtWidgets.QWidget(self) + self.setCentralWidget(central_widget) + main_layout = QtWidgets.QVBoxLayout(central_widget) + + # Create Matplotlib figure and canvas for the bar plot + self.figure = plt.Figure(figsize=(5, 5)) + self.canvas = FigureCanvas(self.figure) + + # Add the canvas with a stretch factor of 9 (90% of the space) + main_layout.addWidget(self.canvas, stretch=9) + + # Add a spacer widget with a stretch factor of 1 (10% of the space) + spacer = QtWidgets.QWidget() + spacer.setSizePolicy(QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Expanding) + main_layout.addWidget(spacer, stretch=1) + + # Create a form layout for the edit boxes (min/max y, target, tolerance) + form_layout = QtWidgets.QFormLayout() + + self.min_y_edit = QtWidgets.QLineEdit("30") + self.max_y_edit = QtWidgets.QLineEdit("80") + self.target_edit = QtWidgets.QLineEdit("60") + self.tolerance_edit = QtWidgets.QLineEdit("5") + + form_layout.addRow("Min Y limit:", self.min_y_edit) + form_layout.addRow("Max Y limit:", self.max_y_edit) + form_layout.addRow("Target (rpm):", self.target_edit) + form_layout.addRow("Tolerance (%):", self.tolerance_edit) + main_layout.addLayout(form_layout) + + # Initialize current average cadence value and out-of-range tracking + self.average_cadence = 0.0 #todo: supprimer + self.out_of_range_start = None + + # Timer to update the plot periodically + #self.update_timer = QtCore.QTimer(self) + #self.update_timer.timeout.connect(self.update_plot) + #self.update_timer.start(100) # update every 100 ms + + def update_value(self, average_cadence: float): + """ + Update the average cadence value. + In your main loop, call: + + feedback_window.update_value(np.mean(self.cadence_array)) + + to supply the current average cadence. + """ + self.average_cadence = average_cadence + + def update_plot(self): + """ + Update the bar plot with the current average cadence. + The bar color is set to: + - Green: if within the tolerance bounds, + - Orange: if outside tolerance for less than 5 seconds, + - Red: if outside tolerance for 5 seconds or more. + """ + # Get current parameters from the edit boxes; if conversion fails, skip update + try: + min_y = float(self.min_y_edit.text()) + max_y = float(self.max_y_edit.text()) + target = float(self.target_edit.text()) + tolerance_pct = float(self.tolerance_edit.text()) + except ValueError: + return + + # Compute tolerance bounds (tolerance as a percentage of the target) + lower_bound = target * (1 - tolerance_pct / 100) + upper_bound = target * (1 + tolerance_pct / 100) + + # Determine the bar color based on the average cadence + if lower_bound <= self.average_cadence <= upper_bound: + color = 'green' + self.out_of_range_start = None # Reset timer if back in tolerance + else: + if self.out_of_range_start is None: + self.out_of_range_start = time.time() + elapsed = time.time() - self.out_of_range_start + # Orange if out-of-range for less than 5 seconds; red otherwise. + color = 'orange' if elapsed < 5 else 'red' + + # Clear the figure and create a new subplot. + self.figure.clf() + ax = self.figure.add_subplot(111) + ax.set_ylim(min_y, max_y) + + # Draw a single bar at x=0 with height equal to the average cadence. + ax.bar(0, self.average_cadence, color=color, width=0.5) + ax.set_xticks([]) # Hide x-axis ticks + + # Draw horizontal lines for target and tolerance bounds. + ax.axhline(target, color='black', linestyle='--', label='Target') + ax.axhline(lower_bound, color='blue', linestyle='--', label='Target - tol') + ax.axhline(upper_bound, color='blue', linestyle='--', label='Target + tol') + #ax.legend(loc='upper right') + + # Optionally add a text label showing the average cadence. + ax.text(0, self.average_cadence, f"{self.average_cadence:.1f} rpm", + ha='center', va='bottom') + + # Refresh the canvas. + self.canvas.draw() diff --git a/ergocycleS2M/motor_control/enums.py b/ergocycleS2M/motor_control/enums.py index 1f75666..8e33e22 100644 --- a/ergocycleS2M/motor_control/enums.py +++ b/ergocycleS2M/motor_control/enums.py @@ -17,6 +17,7 @@ class ControlMode(StrEnum): TORQUE_CONTROL = "Torque control" CONCENTRIC_POWER_CONTROL = "Concentric power control" ECCENTRIC_POWER_CONTROL = "Eccentric power control" + ISOMETRIC_CONTROL = "Isometric control" # Thanks to the StrEnum, we can use the following syntax: diff --git a/ergocycleS2M/motor_control/mock_controller.py b/ergocycleS2M/motor_control/mock_controller.py index 2ae99fb..ba65f2c 100644 --- a/ergocycleS2M/motor_control/mock_controller.py +++ b/ergocycleS2M/motor_control/mock_controller.py @@ -260,6 +260,8 @@ def torque_control( self, user_torque: float = 0.0, torque_ramp_rate: float = 2.0, + gear: int = 0, + hometrainer: int = 0, resisting_torque: float = None, control_mode: ControlMode = ControlMode.TORQUE_CONTROL, ): @@ -311,7 +313,12 @@ def torque_control( return motor_torque # Nm at the pedals def concentric_power_control( - self, power: float = 0.0, torque_ramp_rate: float = 2.0, resisting_torque: float = None + self, + power: float = 0.0, + torque_ramp_rate: float = 2.0, + gear: int = 0, + hometrainer: int = 0, + resisting_torque: float = None, ): """ Ensure a constant power at the pedals. In concentric mode, the power is positive when the user is pedaling and @@ -373,7 +380,14 @@ def eccentric_power_control(self, power: float = 0.0, cadence_ramp_rate: float = cadence = min(abs(power / torque) / 2 / np.pi * 60, cadence_max) return self.cadence_control(cadence, cadence_ramp_rate, ControlMode.ECCENTRIC_POWER_CONTROL) - def linear_control(self, linear_coeff: float = 0.0, torque_ramp_rate: float = 2.0, resisting_torque: float = None): + def linear_control( + self, + linear_coeff: float = 0.0, + torque_ramp_rate: float = 2.0, + gear: int = 0, + hometrainer: int = 0, + resisting_torque: float = None, + ): """ Produce a torque proportional to the user's cadence. diff --git a/ergocycleS2M/motor_control/motor_computations.py b/ergocycleS2M/motor_control/motor_computations.py index 96e2ce6..e4078a4 100644 --- a/ergocycleS2M/motor_control/motor_computations.py +++ b/ergocycleS2M/motor_control/motor_computations.py @@ -23,6 +23,30 @@ def __init__(self, hardware_and_security_path: str = json_path): self.resisting_current_proportional = self.hardware_and_security["resisting_current_proportional"] self.resisting_current_constant = self.hardware_and_security["resisting_current_constant"] + self.prop_gear = self.hardware_and_security["resisting_current_prop_gear"] + self.prop_hometrainer = self.hardware_and_security["resisting_current_prop_hometrainer"] + self.prop_cst = self.hardware_and_security["resisting_current_prop_constant"] + self.cst_gear = self.hardware_and_security["resisting_current_cst_gear"] + self.cst_hometrainer = self.hardware_and_security["resisting_current_cst_hometrainer"] + self.cst_cst = self.hardware_and_security["resisting_current_cst_constant"] + + self.gears = [0, 36, 32, 28, 25, 22, 19, 17, 15, 13, 11] + + def compute_resisting_current_coefficients(self, gear: int = 0, hometrainer: int = 0): + """ """ + if gear == 0: + return ( + self.hardware_and_security["resisting_current_proportional"], + self.hardware_and_security["resisting_current_constant"], + ) + else: + resisting_current_proportional = ( + self.prop_gear / self.gears[gear] + self.prop_hometrainer * hometrainer + self.prop_cst + ) + resisting_current_constant = ( + self.cst_gear / self.gears[gear] + self.cst_hometrainer * hometrainer + self.cst_cst + ) + return resisting_current_proportional, resisting_current_constant @staticmethod def compute_angle(turns: float) -> float: @@ -57,12 +81,14 @@ def compute_cadence(self, vel_estimate: float) -> float: """ return -vel_estimate * self.reduction_ratio * 60 - def compute_resisting_torque_for_positive_velocity(self, vel_estimate: float) -> float: - return np.sign(vel_estimate) * ( - self.resisting_current_proportional * abs(vel_estimate) + self.resisting_current_constant - ) + def compute_resisting_torque_for_positive_velocity(self, vel_estimate: float, gear, hometrainer) -> float: + resisting_current_proportional, resisting_current_constant = self.compute_resisting_current_coefficients(gear, hometrainer) + print(resisting_current_proportional, resisting_current_constant) + return np.sign(vel_estimate) * (resisting_current_proportional * abs(vel_estimate) + resisting_current_constant) - def compute_resisting_current(self, i_measured: float, vel_estimate: float) -> float: + def compute_resisting_current( + self, i_measured: float, vel_estimate: float, gear: int = 0, hometrainer: int = 0 + ) -> float: """ Returns the current corresponding to the resisting torque. @@ -78,16 +104,19 @@ def compute_resisting_current(self, i_measured: float, vel_estimate: float) -> f resisting_current : float The current corresponding to the resisting torque. """ + _, resisting_current_constant = self.compute_resisting_current_coefficients(gear, hometrainer) if vel_estimate != 0.0: - resisting_current = self.compute_resisting_torque_for_positive_velocity(vel_estimate) + resisting_current = self.compute_resisting_torque_for_positive_velocity(vel_estimate, gear, hometrainer) else: # As the motor is not moving, we consider that all the current under the resisting_current_constant is # dissipated in the motor, the rest corresponds to the user torque. This is not what actually happens but - # this choice has been made, in case of the study of a static movement it has to be adapted. - resisting_current = -np.sign(i_measured) * min(self.resisting_current_constant, abs(i_measured)) + # this choice has been made, in case of the study of resisting_current_proportional static movement it has to be adapted. + resisting_current = -np.sign(i_measured) * min(resisting_current_constant, abs(i_measured)) return resisting_current - def compute_resisting_torque(self, i_measured: float, vel_estimate: float) -> float: + def compute_resisting_torque( + self, i_measured: float, vel_estimate: float, gear: int = 0, hometrainer: int = 0 + ) -> float: """ Returns the resisting torque. @@ -104,13 +133,13 @@ def compute_resisting_torque(self, i_measured: float, vel_estimate: float) -> fl resisting_torque : float The resisting torque due to solid frictions in Nm at the pedals. """ - return self.torque_constant * self.compute_resisting_current(i_measured, vel_estimate) / self.reduction_ratio + return ( + self.torque_constant + * self.compute_resisting_current(i_measured, vel_estimate, gear, hometrainer) + / self.reduction_ratio + ) - def compute_user_torque( - self, - i_measured: float, - vel_estimate: float, - ) -> float: + def compute_user_torque(self, i_measured: float, vel_estimate: float, gear: int = 0, hometrainer: int = 0) -> float: """ Returns the measured user torque (the resisting torque is subtracted from the motor torque). @@ -127,7 +156,7 @@ def compute_user_torque( The measured user torque in Nm at the pedals. """ return ( - -self.compute_resisting_torque(i_measured, vel_estimate) + -self.compute_resisting_torque(i_measured, vel_estimate, gear, hometrainer) - self.torque_constant * i_measured / self.reduction_ratio ) diff --git a/ergocycleS2M/motor_control/motor_controller.py b/ergocycleS2M/motor_control/motor_controller.py index 6bda8a5..5c21692 100644 --- a/ergocycleS2M/motor_control/motor_controller.py +++ b/ergocycleS2M/motor_control/motor_controller.py @@ -55,8 +55,8 @@ class MotorController(MotorComputations): """ Represents a motor controlled by an Odrive with the integrated Hall encoder. This script has been written for one - Odrive and one motor TSDZ2 wired on the axis0 of the Odrive. If a motor happens to be wired on axis1 just change the - line `self.axis = self.odrive_board.axis0` to `self.axis = self.odrive_board.axis1` and it should work. + Odrive and one motor TSDZ2 wired on the axis1 of the Odrive. If a motor happens to be wired on axis1 just change the + line `self.axis = self.odrive_board.axis1` to `self.axis = self.odrive_board.axis1` and it should work. """ def __init__( @@ -73,9 +73,12 @@ def __init__( self._watchdog_feed_time = self.hardware_and_security["watchdog_feed_time"] print("Look for an odrive ...") self.odrive_board = odrive.find_any() - # The following line has been written to simplify the occurrences of `self.odrive_board.axis0` in the code and + # The following line has been written to simplify the occurrences of `self.odrive_board.axis1` in the code and # if the motor happened to be wired on axis 1, it would be easier to change it. - self.axis = self.odrive_board.axis0 + self.odrive_board.clear_errors() + self.axis = self.odrive_board.axis1 + self._gains_path = gains_path + self.gains_configuration(False) print("Odrive found") self._watchdog_is_ready = self.config_watchdog(enable_watchdog) @@ -84,8 +87,6 @@ def __init__( self._direction = DirectionMode.FORWARD self.previous_control_mode = ControlMode.STOP - self._gains_path = gains_path - if file_path: self.file_path = file_path else: @@ -104,17 +105,17 @@ def erase_configuration(self) -> None: except fibre.libfibre.ObjectLostError: pass self.odrive_board = odrive.find_any() - # The following line has been written to simplify the occurrences of `self.odrive_board.axis0` in the code and + # The following line has been written to simplify the occurrences of `self.odrive_board.axis1` in the code and # if the motor happened to be wired on axis 1, it would be easier to change it. - self.axis = self.odrive_board.axis0 + self.axis = self.odrive_board.axis1 try: self.odrive_board.reboot() except fibre.libfibre.ObjectLostError: pass self.odrive_board = odrive.find_any() - # The following line has been written to simplify the occurrences of `self.odrive_board.axis0` in the code and + # The following line has been written to simplify the occurrences of `self.odrive_board.axis1` in the code and # if the motor happened to be wired on axis 1, it would be easier to change it. - self.axis = self.odrive_board.axis0 + self.axis = self.odrive_board.axis1 def save_configuration(self) -> None: """ @@ -128,17 +129,17 @@ def save_configuration(self) -> None: except fibre.libfibre.ObjectLostError: pass self.odrive_board = odrive.find_any() - # The following line has been written to simplify the occurrences of `self.odrive_board.axis0` in the code and + # The following line has been written to simplify the occurrences of `self.odrive_board.axis1` in the code and # if the motor happened to be wired on axis 1, it would be easier to change it. - self.axis = self.odrive_board.axis0 + self.axis = self.odrive_board.axis1 try: self.odrive_board.reboot() except fibre.libfibre.ObjectLostError: pass self.odrive_board = odrive.find_any() - # The following line has been written to simplify the occurrences of `self.odrive_board.axis0` in the code and + # The following line has been written to simplify the occurrences of `self.odrive_board.axis1` in the code and # if the motor happened to be wired on axis 1, it would be easier to change it. - self.axis = self.odrive_board.axis0 + self.axis = self.odrive_board.axis1 def config_watchdog(self, enable_watchdog: bool, watchdog_timeout: float = None) -> bool: """ @@ -295,15 +296,11 @@ def gains_configuration( # For position and cadence control if k_vel_gain is not None: self.axis.controller.config.vel_gain = ( - k_vel_gain - * self.axis.motor.config.torque_constant - * self.axis.encoder.config.cpr + k_vel_gain * self.axis.motor.config.torque_constant * self.axis.encoder.config.cpr ) if k_vel_integrator_gain is not None: self.axis.controller.config.vel_integrator_gain = ( - k_vel_integrator_gain - * self.axis.motor.config.torque_constant - * self.axis.encoder.config.cpr + k_vel_integrator_gain * self.axis.motor.config.torque_constant * self.axis.encoder.config.cpr ) # For position, cadence and torque control if current_gain is not None: @@ -326,9 +323,9 @@ def hardware_and_security_configuration(self) -> None: "dc_bus_overvoltage_ramp_start" ] self.odrive_board.config.dc_bus_overvoltage_ramp_end = self.hardware_and_security["dc_bus_overvoltage_ramp_end"] - self.odrive_board.config.gpio9_mode = self.hardware_and_security["gpio9_mode"] - self.odrive_board.config.gpio10_mode = self.hardware_and_security["gpio10_mode"] - self.odrive_board.config.gpio11_mode = self.hardware_and_security["gpio11_mode"] + self.odrive_board.config.gpio12_mode = self.hardware_and_security["gpio9_mode"] + self.odrive_board.config.gpio13_mode = self.hardware_and_security["gpio10_mode"] + self.odrive_board.config.gpio14_mode = self.hardware_and_security["gpio11_mode"] self.odrive_board.config.max_regen_current = self.hardware_and_security["max_regen_current"] self.odrive_board.config.dc_max_positive_current = self.hardware_and_security["dc_max_positive_current"] self.odrive_board.config.dc_max_negative_current = self.hardware_and_security["dc_max_negative_current"] @@ -339,9 +336,7 @@ def hardware_and_security_configuration(self) -> None: self.axis.controller.config.vel_limit = ( self.hardware_and_security["pedals_cadence_limit"] / self.reduction_ratio / 60 ) # tr/s - self.axis.controller.config.vel_limit_tolerance = self.hardware_and_security[ - "vel_limit_tolerance" - ] # tr/s + self.axis.controller.config.vel_limit_tolerance = self.hardware_and_security["vel_limit_tolerance"] # tr/s # Encoder self.axis.encoder.config.mode = self.hardware_and_security["mode"] # Mode of the encoder @@ -349,23 +344,18 @@ def hardware_and_security_configuration(self) -> None: self.axis.encoder.config.calib_scan_distance = self.hardware_and_security["calib_scan_distance"] # Motor + print("here", self.axis.motor.config.I_bus_hard_max) + self.axis.motor.config.requested_current_range = 35.0 + self.axis.motor.config.I_bus_hard_max = self.hardware_and_security["I_bus_hard_max"] self.axis.motor.config.motor_type = self.hardware_and_security["motor_type"] self.axis.motor.config.pole_pairs = self.hardware_and_security["pole_pairs"] self.axis.motor.config.torque_constant = self.hardware_and_security["torque_constant"] self.axis.motor.config.calibration_current = self.hardware_and_security["calibration_current"] - self.axis.motor.config.resistance_calib_max_voltage = self.hardware_and_security[ - "resistance_calib_max_voltage" - ] - self.axis.motor.config.requested_current_range = self.hardware_and_security[ - "requested_current_range" - ] - self.axis.motor.config.current_control_bandwidth = self.hardware_and_security[ - "current_control_bandwidth" - ] + self.axis.motor.config.resistance_calib_max_voltage = self.hardware_and_security["resistance_calib_max_voltage"] + self.axis.motor.config.requested_current_range = self.hardware_and_security["requested_current_range"] + self.axis.motor.config.current_control_bandwidth = self.hardware_and_security["current_control_bandwidth"] self.axis.motor.config.current_lim = self.hardware_and_security["current_lim"] - self.axis.motor.config.torque_lim = ( - self.hardware_and_security["torque_lim"] * self.reduction_ratio - ) + self.axis.motor.config.torque_lim = self.hardware_and_security["torque_lim"] * self.reduction_ratio # cadence and acceleration limits self.axis.trap_traj.config.vel_limit = self.axis.controller.config.vel_limit @@ -524,6 +514,8 @@ def torque_control( self, user_torque: float = 0.0, torque_ramp_rate: float = 2.0, + gear: int = 0, + hometrainer: int = 0, resisting_torque: float = None, control_mode: ControlMode = ControlMode.TORQUE_CONTROL, ) -> float: @@ -571,7 +563,7 @@ def torque_control( if resisting_torque is None: resisting_torque = self.compute_resisting_torque( - self.axis.motor.current_control.Iq_measured, vel_estimate + self.axis.motor.current_control.Iq_measured, vel_estimate, gear, hometrainer ) if user_torque == 0.0: @@ -603,7 +595,12 @@ def torque_control( return self.axis.controller.torque_setpoint / self.reduction_ratio # Nm at the pedals def concentric_power_control( - self, power: float = 0.0, torque_ramp_rate: float = 2.0, resisting_torque: float = None + self, + power: float = 0.0, + torque_ramp_rate: float = 2.0, + gear: int = 0, + hometrainer: int = 0, + resisting_torque: float = None, ) -> float: """ Ensure a constant power at the pedals. In concentric mode, the power is positive when the user is pedaling and @@ -626,11 +623,15 @@ def concentric_power_control( """ cadence = abs(self.axis.encoder.vel_estimate * self.reduction_ratio * 2 * np.pi) # rad/s if cadence == 0: - return self.torque_control(0.0, torque_ramp_rate, resisting_torque, ControlMode.CONCENTRIC_POWER_CONTROL) + return self.torque_control( + 0.0, torque_ramp_rate, gear, hometrainer, resisting_torque, ControlMode.CONCENTRIC_POWER_CONTROL + ) else: return self.torque_control( min(abs(power) / cadence, self.hardware_and_security["torque_lim"]), torque_ramp_rate, + gear, + hometrainer, resisting_torque, ControlMode.CONCENTRIC_POWER_CONTROL, ) @@ -668,7 +669,12 @@ def eccentric_power_control( return self.cadence_control(cadence, cadence_ramp_rate, ControlMode.ECCENTRIC_POWER_CONTROL) def linear_control( - self, linear_coeff: float = 0.0, torque_ramp_rate: float = 2.0, resisting_torque: float = None + self, + linear_coeff: float = 0.0, + torque_ramp_rate: float = 2.0, + gear: int = 0, + hometrainer: int = 0, + resisting_torque: float = None, ) -> float: """ Produce a torque proportional to the user's cadence. @@ -691,10 +697,89 @@ def linear_control( return self.torque_control( min(cadence * abs(linear_coeff), self.hardware_and_security["torque_lim"]), torque_ramp_rate, + gear, + hometrainer, resisting_torque, ControlMode.LINEAR_CONTROL, ) + def isometric_control( + self, + target_angle: float = 0.0, + cadence_ramp_rate: float = 5.0, + control_mode: ControlMode = ControlMode.ISOMETRIC_CONTROL, + ) -> None: + + + if not (-180.0 <= target_angle <= 360.0): + raise ValueError(f"Angle must be in [-180, 360], got {target_angle}.") + + # Compute target position in turns relative to calibration + target_turns = -self.get_sign() *target_angle / 360.0 / self.reduction_ratio + target_pos = self._relative_pos + target_turns # absolute encoder pos + + if self._control_mode != ControlMode.POSITION_CONTROL: + self.stop() + self.axis.controller.config.control_mode = CONTROL_MODE_POSITION_CONTROL + self.axis.controller.config.input_mode = INPUT_MODE_TRAP_TRAJ + self.axis.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL + + self.axis.controller.input_pos = target_pos + self._control_mode = ControlMode.POSITION_CONTROL + + + """ + Control the crank to maintain an isometric position. + If deviation > 5°, slowly move at 5 rpm toward the target. + """ + + + + + """ + current_angle = self.get_angle() + error = target_angle - current_angle # signed error + + + current_turn = -(self.axis.encoder.pos_estimate - self._relative_pos) * self.reduction_ratio + print(f"Current turn: {current_turn:.2f}") + + + + + + print(f"Current angle: {current_angle:.2f}°, Target angle: {target_angle:.2f}°, Error: {error:.2f}°") + + if abs(error) > 5: + # Move in the direction of the error at 5 rpm + direction = np.sign(error) # 1 if target ahead, -1 if behind + velocity_rpm = 5 * direction + else: + # No motion needed + velocity_rpm = 0 + + print(velocity_rpm) + + # Convert to motor units + velocity_tr_per_s = velocity_rpm / 60 / self.reduction_ratio + ramp_rate_tr_per_s2 = cadence_ramp_rate / 60 / self.reduction_ratio + + # Apply velocity control + if self._control_mode not in control_modes_based_on_cadence: + self.stopping() + self.stopped() + self.axis.controller.config.control_mode = CONTROL_MODE_VELOCITY_CONTROL + self.axis.controller.config.input_mode = INPUT_MODE_VEL_RAMP + self.axis.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL + + self.axis.controller.config.vel_ramp_rate = ramp_rate_tr_per_s2 + self.axis.controller.input_vel = -self.get_sign() * velocity_tr_per_s + self._control_mode = control_mode + + return error""" + + + def stopping( self, cadence_ramp_rate: float = 30, @@ -816,13 +901,15 @@ def get_motor_torque(self) -> float: """ return self.compute_motor_torque(self.axis.motor.current_control.Iq_measured) - def get_resisting_torque(self) -> float: + def get_resisting_torque(self, gear=0, hometrainer=0) -> float: """ Returns the resisting torque. """ return self.compute_resisting_torque( self.axis.motor.current_control.Iq_measured, self.axis.encoder.vel_estimate, + gear, + hometrainer, ) def get_user_torque(self) -> float: diff --git a/ergocycleS2M/parameters/gains.json b/ergocycleS2M/parameters/gains.json index 076b783..458c937 100644 --- a/ergocycleS2M/parameters/gains.json +++ b/ergocycleS2M/parameters/gains.json @@ -1,6 +1,6 @@ { "pos_gain": 1.0, - "k_vel_gain": 0.02, + "k_vel_gain": 0.01, "k_vel_integrator_gain": 0.1, "bandwidth": 100.0 -} \ No newline at end of file +} diff --git a/ergocycleS2M/parameters/hardware_and_security.json b/ergocycleS2M/parameters/hardware_and_security.json index 4320524..2a9eab8 100644 --- a/ergocycleS2M/parameters/hardware_and_security.json +++ b/ergocycleS2M/parameters/hardware_and_security.json @@ -22,15 +22,22 @@ "resistance_calib_max_voltage": 20.0, "requested_current_range": 25.0, "current_control_bandwidth": 100.0, - "current_lim": 10.416666666666666, - "torque_lim": 38.75160393469862, - "power_lim": 267.8318593193551, + "I_bus_hard_max": 12.5, + "current_lim": 30, + "torque_lim": 223.20923866386406, + "power_lim": 1542.7115096794855, "watchdog_timeout": 0.3, "watchdog_feed_time": 0.01, "reduction_ratio": 0.02442002442002442, "pedals_accel_lim": 31, "torque_ramp_rate_lim": 5.5, "maximal_cadence_stop": 31, - "resisting_current_proportional": 0.012109769054928311, - "resisting_current_constant": 0.38324651511638025 + "resisting_current_proportional": 0.004312443183247655, + "resisting_current_constant": 0.3251453534100984, + "resisting_current_cst_gear": 26.865107332479884, + "resisting_current_cst_hometrainer": 0.0751487381089588, + "resisting_current_cst_constant": -0.4242291122064278, + "resisting_current_prop_gear": 2.912417063523588, + "resisting_current_prop_hometrainer": 0.020903324203691103, + "resisting_current_prop_constant": -0.18166617262949072 } \ No newline at end of file diff --git a/ergocycleS2M/parameters/write_to_json.py b/ergocycleS2M/parameters/write_to_json.py index 3088c92..047f8d4 100644 --- a/ergocycleS2M/parameters/write_to_json.py +++ b/ergocycleS2M/parameters/write_to_json.py @@ -6,7 +6,7 @@ with open("hardware_and_security.json", "r") as hardware_and_security_file: hardware_and_security = json.load(hardware_and_security_file) -current_lim = 500 / 48 # 500W / 48V # Not sure of this value +current_lim = 60 # A # Calculated with the `torque_constant_computation.py` script # 0.1053225104205947 (calib done by Amandine at the beginning of her internship) # 0.09084625098244366 (calib done by Amandine and Kevin at the end of her internship with greater loads) @@ -56,6 +56,7 @@ "resistance_calib_max_voltage": 20.0, # Not sure of this value "requested_current_range": 25.0, # > current_lim + current_lim_margin but as little as possible "current_control_bandwidth": 100.0, # Not sure of this value + "I_bus_hard_max": 600 / 48, # 600W / 48V "current_lim": current_lim, "torque_lim": torque_lim, "power_lim": torque_lim * cadence_lim * 2 * np.pi / 60, # Motor power limit in W @@ -70,6 +71,14 @@ # `resisting_current_calibration.py` script (it runs a calibration on the motor that lasts several minutes) "resisting_current_proportional": hardware_and_security["resisting_current_proportional"], "resisting_current_constant": hardware_and_security["resisting_current_constant"], + "resisting_current_proportional": hardware_and_security["resisting_current_proportional"], + "resisting_current_constant": hardware_and_security["resisting_current_constant"], + "resisting_current_cst_gear": hardware_and_security["resisting_current_cst_gear"], + "resisting_current_cst_hometrainer": hardware_and_security["resisting_current_cst_hometrainer"], + "resisting_current_cst_constant": hardware_and_security["resisting_current_cst_constant"], + "resisting_current_prop_gear": hardware_and_security["resisting_current_prop_gear"], + "resisting_current_prop_hometrainer": hardware_and_security["resisting_current_prop_hometrainer"], + "resisting_current_prop_constant": hardware_and_security["resisting_current_prop_constant"] } # Serializing json diff --git a/main.py b/main.py index 210aaa1..2685de7 100644 --- a/main.py +++ b/main.py @@ -6,4 +6,4 @@ if __name__ == "__main__": save_period = 0.1 # seconds app = Application(save_period=save_period) - app.start() + app.start() \ No newline at end of file diff --git a/resisting_current_calibration_load.py b/resisting_current_calibration_load.py new file mode 100644 index 0000000..b65944b --- /dev/null +++ b/resisting_current_calibration_load.py @@ -0,0 +1,126 @@ +""" +This script is used to sample the current corresponding to the resisting torque of the motor at different velocities. +60 psi +""" +import json +import matplotlib.pyplot as plt +import numpy as np +import time + +from scipy.optimize import curve_fit + +from ergocycleS2M.motor_control.enums import DirectionMode +from ergocycleS2M.motor_control.motor_controller import MotorController + +# Initialisation +motor = MotorController() +motor.set_direction(DirectionMode.REVERSE) + + +def calibration(instruction, nb_turns=5): + """ + This function is used to sample the current corresponding to the resisting torque of the motor at different + cadences. + + Parameters + ---------- + instruction + The cadence instruction in rpm. + nb_turns + The number of turns to sample the data at the desired cadence. + """ + if instruction > 0: + motor.set_direction(DirectionMode.FORWARD) + else: + motor.set_direction(DirectionMode.REVERSE) + motor.cadence_control(instruction) + + print(f"Waiting to stabilize at {instruction} rpm...") + + # Wait for to reach the instruction + while not (instruction - 1 < motor.get_cadence() < instruction + 1): + pass + + t0 = time.time() + t1 = time.time() + + # Wait for 1 second to stabilize at the instructed velocity (can't be done with a time.sleep() because of the + # watchdog thread) + while t1 - t0 < 3.0: + t1 = time.time() + + print(f"Stabilized at {instruction} rpm.") + + t0 = time.time() + t1 = time.time() + + # Save the data as frequently as possible during nb_turns + iq_measured = [] + vel_estimate = [] + while t1 - t0 < nb_turns / (abs(instruction) / 60): + t1 = time.time() + iq_measured.append(motor.axis.motor.current_control.Iq_measured) + vel_estimate.append(motor.axis.encoder.vel_estimate) + print(np.mean(iq_measured), "*/-", np.std(iq_measured), "A") + print(np.mean(vel_estimate), "*/-", np.std(vel_estimate), "tr/s") + return np.mean(iq_measured), np.std(iq_measured), np.mean(vel_estimate), np.std(vel_estimate) + + +if __name__ == "__main__": + intensities = [] + velocities = [] + intensities_std = [] + velocities_std = [] + for ins in range(5, 61, 5): + average_iq_measured, i_std, average_vel_estimate, v_std = calibration(ins) + intensities.append(average_iq_measured) + velocities.append(average_vel_estimate) + intensities_std.append(i_std) + velocities_std.append(v_std) + + def lost_current(vel_estimate, resisting_current_proportional, resisting_current_constant): + """ + The current corresponding to the resisting torque of the motor is modeled as a linear function of the velocity. + + Parameters + ---------- + vel_estimate: + The velocity of the motor. + resisting_current_proportional: + The proportional coefficient. + resisting_current_constant: + The constant coefficient. + + Returns + ------- + + """ + return np.sign(vel_estimate) * ( + resisting_current_proportional * np.abs(vel_estimate) + resisting_current_constant + ) + + popt = curve_fit(lost_current, np.asarray(velocities), np.asarray(intensities), p0=[0.01, 0.5]) + + motor.stop() + + plt.plot(velocities, intensities) + plt.plot(velocities, lost_current(velocities, *popt[0])) + plt.xlabel("Motor velocity (tr/s)") + plt.ylabel("Resisting current (A)") + plt.show() + + data = { + "velocities": velocities, + "intensities": intensities, + "velocities_std": velocities_std, + "intensities": intensities_std, + "a" : -popt[0][0], + "b" : -popt[0][1] + } + + print(data["a"], data["b"]) + + # Writing to .json + json_object = json.dumps(data, indent=4) + with open("./calibration_files/resisting_current_load_h3_b10.json", "w") as outfile: + outfile.write(json_object)