Revision 0:9cf3812720c0, committed 2018-06-26

Comitter:

AlanHuchin

Date:

Tue Jun 26 18:46:55 2018 +0000

Commit message:

pendulo_invertido

Changed in this revision

diff -r 000000000000 -r 9cf3812720c0 Servo.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo.lib	Tue Jun 26 18:46:55 2018 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/simon/code/Servo/#36b69a7ced07

diff -r 000000000000 -r 9cf3812720c0 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Jun 26 18:46:55 2018 +0000
@@ -0,0 +1,161 @@
+#include "mbed.h"
+#include "Servo.h"
+#include "rtos.h"
+
+Serial pc(USBTX, USBRX);
+Servo esc1(p23);
+InterruptIn pb1(p21);
+InterruptIn pb2(p22);
+AnalogIn ain(p20); 
+
+Timer t;
+Thread thread1;
+Thread thread2;
+Thread thread3;
+
+void flip_pb1();
+void flip_pb2();
+void read_analog();
+float map(float in, float inMin, float inMax, float outMin, float outMax);
+float multiMap(float val, float* _in, float* _out, int size);
+void error();
+void incremento();
+
+float cnt = 0.0, grados = 0.0, adc = 0.0, e = 0.0, time_ = 0.0;
+int var = 0;
+
+float out[473] = {1.57992600000000,0.667625000000000,1.28965000000000,1.74579600000000,2.28487400000000,2.65808900000000,3.07276900000000,4.23387100000000,5.22910300000000,4.64855200000000,5.39497400000000,5.64378400000000,5.89258600000000,7.21955900000000,7.09516100000000,7.30250500000000,7.46837600000000,7.55130800000000,8.13185900000000,7.96598800000000,8.21479800000000,8.79534900000000,8.25626400000000,8.29772900000000,8.38066900000000,8.21479800000000,8.96122000000000,8.21479800000000,8.54653900000000,8.13185900000000,8.38066900000000,8.13185900000000,8.13185900000000,8.00745400000000,9.29296100000000,8.04892000000000,8.17333200000000,8.13185900000000,8.09039300000000,8.09039300000000,8.54653900000000,8.21479800000000,8.96122000000000,8.67094400000000,8.67094400000000,8.87828100000000,8.91975400000000,8.83681500000000,9.21002200000000,9.21002200000000,9.99791700000000,9.29296100000000,9.58323700000000,9.87351200000000,9.91498600000000,10.4540630000000,9.91498600000000,10.4540630000000,10.3711320000000,10.6199420000000,10.8687440000000,10.7858120000000,11.2004930000000,11.7810360000000,11.6981050000000,12.2786560000000,11.5322340000000,11.7810360000000,11.8639760000000,12.3201290000000,12.0713120000000,12.7762760000000,12.1127850000000,12.1957170000000,12.4030610000000,11.8639760000000,12.5689320000000,12.1957170000000,12.5274660000000,12.7762760000000,12.0298460000000,12.0298460000000,12.1127850000000,12.1542510000000,12.5274660000000,12.3615950000000,12.6933360000000,11.8639760000000,12.5274660000000,12.6518710000000,12.6518710000000,12.7762760000000,12.8592070000000,13.1080170000000,13.0665510000000,12.9006730000000,13.3568190000000,13.3568190000000,13.5641630000000,13.6885680000000,14.6008610000000,14.2276460000000,15.1814190000000,14.6008610000000,15.2228850000000,14.9326020000000,15.1814190000000,16.0107730000000,15.8449020000000,16.0937120000000,16.8401340000000,16.4254530000000,16.5913240000000,17.8353650000000,17.5036160000000,18.2915120000000,17.9597630000000,18.1671070000000,18.3744430000000,19.2038040000000,18.9135360000000,19.9502260000000,19.0793990000000,19.3282090000000,19.2867430000000,19.2038040000000,19.5770190000000,19.4111480000000,20.0746310000000,19.8258290000000,20.0746310000000,20.1161040000000,19.9502260000000,20.3234410000000,20.5722500000000,20.4893110000000,21.4845430000000,21.1528020000000,21.0698620000000,21.5674820000000,21.7333530000000,21.8992230000000,22.1894990000000,22.5627140000000,22.8944550000000,22.8944550000000,22.6871190000000,22.8115160000000,23.3091280000000,23.2261960000000,23.2676620000000,23.7238080000000,24.0970230000000,23.8067470000000,23.9311520000000,23.9726180000000,24.5946430000000,25.4654690000000,24.9263840000000,24.8849180000000,25.3825300000000,25.4654690000000,25.7972110000000,26.5851020000000,26.7924420000000,26.9168470000000,26.4192310000000,26.5436330000000,26.2533610000000,26.0874860000000,26.2533610000000,25.8801500000000,25.9630890000000,26.0874860000000,26.3362920000000,26.5436330000000,26.3777620000000,26.2533610000000,26.3777620000000,27.4973950000000,26.9583130000000,26.9583130000000,27.6218030000000,27.6632690000000,27.7876740000000,27.9120750000000,28.4926300000000,28.6999700000000,28.4511600000000,29.4463920000000,28.8243750000000,29.1146510000000,29.1561160000000,29.6951980000000,30.8977700000000,29.9025380000000,30.1928180000000,30.3172190000000,30.5245590000000,31.1051100000000,31.3539200000000,31.7271310000000,31.4783250000000,31.8100660000000,32.8882330000000,32.8052980000000,33.6346550000000,33.4687880000000,33.4273150000000,33.4687880000000,34.0078660000000,33.8419950000000,33.8005260000000,34.4225460000000,34.0493350000000,35.6251140000000,34.5469470000000,34.4225460000000,35.3348430000000,34.9616280000000,36.6618160000000,35.6251140000000,35.9983290000000,37.0764960000000,36.2056690000000,37.5326460000000,36.2886050000000,36.2886050000000,36.7032850000000,36.7862240000000,36.9520910000000,37.3253060000000,37.6155780000000,37.2423710000000,37.3667720000000,37.2423710000000,38.2375980000000,38.4034730000000,37.7814520000000,39.9792630000000,38.2790640000000,38.4864040000000,39.1913720000000,39.3987120000000,41.3062400000000,39.7719230000000,39.8548580000000,39.9792630000000,40.4768790000000,40.8086240000000,40.7671590000000,42.3014720000000,41.3477060000000,41.5135800000000,41.8453250000000,41.7209210000000,43.2967000000000,42.2599980000000,43.1722980000000,42.9649580000000,44.2504620000000,43.3796350000000,43.8772540000000,44.6236760000000,44.4163360000000,44.4992750000000,44.7480810000000,44.8310170000000,46.5726700000000,46.6970750000000,46.7800100000000,45.9506490000000,46.3238640000000,46.6970750000000,46.8214760000000,48.1484530000000,46.6970750000000,47.3605610000000,47.1946910000000,47.6923070000000,47.8996470000000,48.4801940000000,49.1436840000000,48.3143230000000,48.5216640000000,49.0607490000000,48.6875340000000,49.8486400000000,49.7657050000000,50.0145110000000,50.3462520000000,50.2218510000000,50.0559810000000,50.5535930000000,51.3000180000000,50.6365320000000,51.8391040000000,51.3000180000000,52.4611210000000,52.2537800000000,52.7099300000000,53.4563520000000,53.1246070000000,53.8295630000000,54.6174550000000,53.9539680000000,54.9906650000000,54.5759890000000,55.1150700000000,55.5712200000000,56.1103020000000,56.4835130000000,57.3958090000000,57.6031490000000,57.9763570000000,57.7275510000000,58.5154420000000,59.3862690000000,59.3862690000000,59.1374630000000,60.3815000000000,59.2203980000000,59.6765440000000,59.8424150000000,60.5473710000000,61.5840720000000,61.4596670000000,61.2937970000000,61.5426030000000,61.6255420000000,61.9572830000000,62.6207730000000,62.7866440000000,63.4501300000000,63.0769200000000,63.2013240000000,63.4916000000000,64.4453580000000,64.5282970000000,65.2747190000000,64.6941680000000,64.9844440000000,64.9844440000000,65.3576510000000,66.3528900000000,66.0626140000000,67.1822510000000,66.6846310000000,67.0578460000000,68.2189480000000,68.1774750000000,68.5921550000000,68.7165600000000,69.8361970000000,70.4167480000000,70.0020680000000,70.7070240000000,70.9143680000000,71.2875750000000,71.7022550000000,72.1169360000000,72.6145480000000,73.5683140000000,73.9415280000000,73.6927190000000,73.8171230000000,74.2317960000000,75.0196910000000,75.3099670000000,75.6831820000000,77.3004300000000,76.0978550000000,76.5954740000000,76.6369400000000,77.2589650000000,78.3371280000000,77.7565770000000,77.9639130000000,78.3371280000000,78.0468520000000,78.3785930000000,78.4615330000000,78.6688690000000,79.8299710000000,79.4567640000000,79.4982300000000,79.3738250000000,80.0787810000000,79.7885060000000,80.5764010000000,80.1202550000000,80.5764010000000,80.2446520000000,80.8666760000000,80.9910810000000,80.9910810000000,80.9496080000000,81.1984180000000,81.2813570000000,82.1521840000000,82.0692440000000,83.2303470000000,82.4424590000000,82.6497960000000,83.6450270000000,83.0644760000000,83.1888810000000,84.0597080000000,83.7279660000000,84.1841130000000,84.3914490000000,84.7231900000000,85.1793440000000,85.7598950000000,85.8013610000000,86.0916370000000,86.2160420000000,86.5892560000000,86.8795320000000,86.9209980000000,87.9162290000000,87.1698070000000,87.2112730000000,87.5015490000000,87.5015490000000,88.3309100000000,87.5015490000000,87.4186100000000,87.6674190000000,87.7503590000000,87.8747560000000,87.9576950000000,87.8332900000000,88.0821000000000,88.3723750000000,87.9162290000000,87.9991610000000,88.1650310000000,88.2894360000000,88.7870560000000,88.4553070000000,88.9529270000000,88.6211850000000,88.8285220000000,89.3676070000000,88.9114610000000,88.7870560000000,88.8699870000000,89.2017360000000,90,89.8237530000000,89.7408140000000,89.1187970000000,89.2017360000000,89.2432020000000,89.1602630000000,89.2432020000000,89.3261410000000,89.4505390000000,89.3261410000000,89.4505390000000,89.4090730000000,89.6578830000000,89.6164170000000,89.8652190000000,89.5749440000000,89.6993480000000,89.8237530000000,89.9066930000000,89.9896240000000,90,89.8652190000000,90};
+float in[473] = {0.0480000000000000,0.0480000000000000,0.0490000000000000,0.0490000000000000,0.0490000000000000,0.0500000000000000,0.0500000000000000,0.0500000000000000,0.0500000000000000,0.0510000000000000,0.0510000000000000,0.0510000000000000,0.0510000000000000,0.0520000000000000,0.0520000000000000,0.0520000000000000,0.0520000000000000,0.0530000000000000,0.0530000000000000,0.0530000000000000,0.0530000000000000,0.0540000000000000,0.0540000000000000,0.0540000000000000,0.0550000000000000,0.0550000000000000,0.0550000000000000,0.0550000000000000,0.0560000000000000,0.0560000000000000,0.0560000000000000,0.0560000000000000,0.0570000000000000,0.0570000000000000,0.0570000000000000,0.0570000000000000,0.0580000000000000,0.0580000000000000,0.0580000000000000,0.0580000000000000,0.0590000000000000,0.0590000000000000,0.0590000000000000,0.0600000000000000,0.0600000000000000,0.0600000000000000,0.0600000000000000,0.0610000000000000,0.0610000000000000,0.0610000000000000,0.0610000000000000,0.0620000000000000,0.0620000000000000,0.0620000000000000,0.0620000000000000,0.0630000000000000,0.0630000000000000,0.0630000000000000,0.0630000000000000,0.0640000000000000,0.0640000000000000,0.0640000000000000,0.0640000000000000,0.0650000000000000,0.0650000000000000,0.0650000000000000,0.0660000000000000,0.0660000000000000,0.0660000000000000,0.0660000000000000,0.0670000000000000,0.0670000000000000,0.0670000000000000,0.0670000000000000,0.0680000000000000,0.0680000000000000,0.0680000000000000,0.0680000000000000,0.0690000000000000,0.0690000000000000,0.0690000000000000,0.0690000000000000,0.0700000000000000,0.0700000000000000,0.0700000000000000,0.0710000000000000,0.0710000000000000,0.0710000000000000,0.0710000000000000,0.0720000000000000,0.0720000000000000,0.0720000000000000,0.0720000000000000,0.0730000000000000,0.0730000000000000,0.0730000000000000,0.0730000000000000,0.0740000000000000,0.0740000000000000,0.0740000000000000,0.0740000000000000,0.0750000000000000,0.0750000000000000,0.0750000000000000,0.0760000000000000,0.0760000000000000,0.0760000000000000,0.0760000000000000,0.0770000000000000,0.0770000000000000,0.0770000000000000,0.0770000000000000,0.0780000000000000,0.0780000000000000,0.0780000000000000,0.0780000000000000,0.0790000000000000,0.0790000000000000,0.0790000000000000,0.0790000000000000,0.0800000000000000,0.0800000000000000,0.0800000000000000,0.0800000000000000,0.0810000000000000,0.0810000000000000,0.0810000000000000,0.0820000000000000,0.0820000000000000,0.0820000000000000,0.0820000000000000,0.0830000000000000,0.0830000000000000,0.0830000000000000,0.0830000000000000,0.0840000000000000,0.0840000000000000,0.0840000000000000,0.0840000000000000,0.0850000000000000,0.0850000000000000,0.0850000000000000,0.0850000000000000,0.0860000000000000,0.0860000000000000,0.0860000000000000,0.0870000000000000,0.0870000000000000,0.0870000000000000,0.0870000000000000,0.0880000000000000,0.0880000000000000,0.0880000000000000,0.0880000000000000,0.0890000000000000,0.0890000000000000,0.0890000000000000,0.0890000000000000,0.0900000000000000,0.0900000000000000,0.0900000000000000,0.0900000000000000,0.0910000000000000,0.0910000000000000,0.0910000000000000,0.0920000000000000,0.0920000000000000,0.0920000000000000,0.0920000000000000,0.0930000000000000,0.0930000000000000,0.0930000000000000,0.0930000000000000,0.0940000000000000,0.0940000000000000,0.0940000000000000,0.0940000000000000,0.0950000000000000,0.0950000000000000,0.0950000000000000,0.0950000000000000,0.0960000000000000,0.0960000000000000,0.0960000000000000,0.0960000000000000,0.0970000000000000,0.0970000000000000,0.0970000000000000,0.0980000000000000,0.0980000000000000,0.0980000000000000,0.0980000000000000,0.0990000000000000,0.0990000000000000,0.0990000000000000,0.0990000000000000,0.100000000000000,0.100000000000000,0.100000000000000,0.100000000000000,0.101000000000000,0.101000000000000,0.101000000000000,0.101000000000000,0.102000000000000,0.102000000000000,0.102000000000000,0.103000000000000,0.103000000000000,0.103000000000000,0.103000000000000,0.104000000000000,0.104000000000000,0.104000000000000,0.104000000000000,0.105000000000000,0.105000000000000,0.105000000000000,0.105000000000000,0.106000000000000,0.106000000000000,0.106000000000000,0.106000000000000,0.107000000000000,0.107000000000000,0.107000000000000,0.108000000000000,0.108000000000000,0.108000000000000,0.108000000000000,0.109000000000000,0.109000000000000,0.109000000000000,0.109000000000000,0.110000000000000,0.110000000000000,0.110000000000000,0.110000000000000,0.111000000000000,0.111000000000000,0.111000000000000,0.111000000000000,0.112000000000000,0.112000000000000,0.112000000000000,0.112000000000000,0.113000000000000,0.113000000000000,0.113000000000000,0.114000000000000,0.114000000000000,0.114000000000000,0.114000000000000,0.115000000000000,0.115000000000000,0.115000000000000,0.115000000000000,0.116000000000000,0.116000000000000,0.116000000000000,0.116000000000000,0.117000000000000,0.117000000000000,0.117000000000000,0.117000000000000,0.118000000000000,0.118000000000000,0.118000000000000,0.119000000000000,0.119000000000000,0.119000000000000,0.119000000000000,0.120000000000000,0.120000000000000,0.120000000000000,0.120000000000000,0.121000000000000,0.121000000000000,0.121000000000000,0.121000000000000,0.122000000000000,0.122000000000000,0.122000000000000,0.122000000000000,0.123000000000000,0.123000000000000,0.123000000000000,0.124000000000000,0.124000000000000,0.124000000000000,0.124000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.125000000000000,0.126000000000000,0.126000000000000,0.126000000000000,0.126000000000000,0.127000000000000,0.127000000000000,0.127000000000000,0.127000000000000,0.128000000000000,0.128000000000000,0.128000000000000,0.128000000000000,0.129000000000000,0.129000000000000,0.129000000000000,0.130000000000000,0.130000000000000,0.130000000000000,0.130000000000000,0.131000000000000,0.131000000000000,0.131000000000000,0.131000000000000,0.132000000000000,0.132000000000000,0.132000000000000,0.132000000000000,0.133000000000000,0.133000000000000,0.133000000000000,0.133000000000000,0.134000000000000,0.134000000000000,0.134000000000000,0.135000000000000,0.135000000000000,0.135000000000000,0.135000000000000,0.136000000000000,0.136000000000000,0.136000000000000,0.136000000000000,0.137000000000000,0.137000000000000,0.137000000000000,0.137000000000000,0.138000000000000,0.138000000000000,0.138000000000000,0.138000000000000,0.139000000000000,0.139000000000000,0.139000000000000,0.140000000000000,0.140000000000000,0.140000000000000,0.140000000000000,0.141000000000000,0.141000000000000,0.141000000000000,0.141000000000000,0.142000000000000,0.142000000000000,0.142000000000000,0.142000000000000,0.143000000000000,0.143000000000000,0.143000000000000,0.143000000000000,0.144000000000000,0.144000000000000,0.144000000000000,0.144000000000000,0.145000000000000,0.145000000000000,0.145000000000000,0.146000000000000,0.146000000000000,0.146000000000000,0.146000000000000,0.147000000000000,0.147000000000000,0.147000000000000,0.147000000000000,0.148000000000000,0.148000000000000,0.148000000000000,0.148000000000000,0.149000000000000,0.149000000000000,0.149000000000000,0.149000000000000,0.150000000000000,0.150000000000000,0.150000000000000,0.151000000000000,0.151000000000000,0.151000000000000,0.151000000000000,0.152000000000000,0.152000000000000,0.152000000000000,0.152000000000000,0.153000000000000,0.153000000000000,0.153000000000000,0.153000000000000,0.154000000000000,0.154000000000000,0.154000000000000,0.154000000000000,0.155000000000000,0.155000000000000,0.155000000000000,0.156000000000000,0.156000000000000,0.156000000000000,0.156000000000000,0.157000000000000,0.157000000000000,0.157000000000000,0.157000000000000,0.158000000000000,0.158000000000000,0.158000000000000,0.158000000000000,0.159000000000000,0.159000000000000,0.159000000000000,0.159000000000000,0.160000000000000,0.160000000000000,0.160000000000000,0.160000000000000,0.161000000000000,0.161000000000000,0.161000000000000,0.162000000000000,0.162000000000000,0.162000000000000,0.162000000000000,0.163000000000000,0.163000000000000,0.163000000000000,0.163000000000000,0.164000000000000,0.164000000000000,0.164000000000000,0.164000000000000,0.165000000000000,0.165000000000000,0.165000000000000,0.165000000000000,0.166000000000000,0.166000000000000,0.166000000000000,0.167000000000000,0.167000000000000,0.167000000000000,0.167000000000000,0.168000000000000,0.168000000000000,0.168000000000000,0.168000000000000,0.169000000000000,0.169000000000000,0.169000000000000,0.169000000000000,0.170000000000000,0.170000000000000,0.170000000000000,0.170000000000000,0.171000000000000,0.171000000000000,0.171000000000000,0.172000000000000,0.172000000000000,0.172000000000000};
+float z = 0.0;
+
+int main() {
+            
+    pc.baud(115200);
+    pc.printf("-------------------------------------------------- \n\r");
+    
+    esc1 = 0.0;
+    wait(5.0); 
+    
+    pc.printf("Motores Activados\n\r" );
+    pc.printf("Todo Listo...\n\r" );
+    wait(1.0);
+    
+    pb1.rise(&flip_pb1); 
+    pb2.rise(&flip_pb2);
+    thread1.start(read_analog);
+    thread2.start(error);
+    thread3.start(incremento);
+    
+    var = 1;
+    t.start();
+    while(1){
+    
+        if(var == 1){
+            //pc.printf("adc: %f, grad: %f, pwm: %f, e: %f, pwm_des:%f\n", adc, grados, cnt, e, z );
+            pc.printf("%f, %f, %f \n", grados, t.read(),60.0);
+            //pc.printf("%f, %f \n", grados, cnt);
+        }
+                
+        Thread::wait(20); 
+    }
+
+}
+
+void flip_pb1() {
+  
+    wait_ms(50);
+    cnt = cnt + 0.01;
+    if (cnt > 1.0){cnt = 1.0;}
+  
+}
+
+
+void flip_pb2() {
+    
+    wait_ms(50);
+    cnt = cnt - 0.01;
+    if (cnt < 0.0){cnt = 0.0;}
+       
+}
+
+
+void read_analog(){
+     while(true){
+        
+        adc = ain.read();
+        grados = map(adc,0.45,0.98,90.0,0.0);
+        e = ( 45.0 - grados); 
+        
+        
+        Thread::wait(10);  
+        
+    }
+}
+
+void error(){
+     
+     while(true){
+        esc1 = z;         
+        Thread::wait(20); 
+         
+    }
+}
+
+void incremento(){
+    
+    while(true){
+        
+        if(var == 1){
+            z =  multiMap(60, out, in, 473);
+            if (t > 10.0){
+                t.reset();
+                t.stop();
+                cnt = 0.0;
+                var = 0;
+                z=0.0;
+            }  
+        }
+        
+        Thread::wait(5);  
+    }
+
+}
+
+float map(float in, float inMin, float inMax, float outMin, float outMax) {
+  // check it's within the range
+  if (inMin<inMax) { 
+    if (in <= inMin) 
+      return outMin;
+    if (in >= inMax)
+      return outMax;
+  } else {  // cope with input range being backwards.
+    if (in >= inMin) 
+      return outMin;
+    if (in <= inMax)
+      return outMax;
+  }
+  // calculate how far into the range we are
+  float scale = (in-inMin)/(inMax-inMin);
+  // calculate the output.
+  return outMin + scale*(outMax-outMin);
+}
+
+float multiMap(float val, float* _in, float* _out, int size)
+{
+  // take care the value is within range
+  // val = constrain(val, _in[0], _in[size-1]);
+  if (val <= _in[0]) return _out[0];
+  if (val >= _in[size-1]) return _out[size-1];
+  
+  // search right interval
+  int pos = 1;  // _in[0] allready tested
+  while(val > _in[pos]){
+      pos++;
+  }
+
+  // this will handle all exact "points" in the _in array
+  if (val == _in[pos]) return _out[pos];
+
+  // interpolate in the right segment for the rest
+  return (val - _in[pos-1]) * (_out[pos] - _out[pos-1]) / (_in[pos] - _in[pos-1]) + _out[pos-1];
+}

diff -r 000000000000 -r 9cf3812720c0 mbed-rtos.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-rtos.lib	Tue Jun 26 18:46:55 2018 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed-rtos/#5713cbbdb706

diff -r 000000000000 -r 9cf3812720c0 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Jun 26 18:46:55 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/5aab5a7997ee
\ No newline at end of file