Alan Huchin Herrera / Mbed 2 deprecated mbed_LPC1768_Pendulo_rtos

Banco de pruebas péndulo invertido

Dependencies:   Servo mbed-rtos mbed

main.cpp@0:9cf3812720c0, 2018-06-26 (annotated)

Committer:
AlanHuchin
Date:
Tue Jun 26 18:46:55 2018 +0000
Revision:
0:9cf3812720c0
pendulo_invertido

Who changed what in which revision?

UserRevisionLine numberNew contents of line
AlanHuchin 0:9cf3812720c0 1 #include "mbed.h"
AlanHuchin 0:9cf3812720c0 2 #include "Servo.h"
AlanHuchin 0:9cf3812720c0 3 #include "rtos.h"
AlanHuchin 0:9cf3812720c0 4
AlanHuchin 0:9cf3812720c0 5 Serial pc(USBTX, USBRX);
AlanHuchin 0:9cf3812720c0 6 Servo esc1(p23);
AlanHuchin 0:9cf3812720c0 7 InterruptIn pb1(p21);
AlanHuchin 0:9cf3812720c0 8 InterruptIn pb2(p22);
AlanHuchin 0:9cf3812720c0 9 AnalogIn ain(p20);
AlanHuchin 0:9cf3812720c0 10
AlanHuchin 0:9cf3812720c0 11 Timer t;
AlanHuchin 0:9cf3812720c0 12 Thread thread1;
AlanHuchin 0:9cf3812720c0 13 Thread thread2;
AlanHuchin 0:9cf3812720c0 14 Thread thread3;
AlanHuchin 0:9cf3812720c0 15
AlanHuchin 0:9cf3812720c0 16 void flip_pb1();
AlanHuchin 0:9cf3812720c0 17 void flip_pb2();
AlanHuchin 0:9cf3812720c0 18 void read_analog();
AlanHuchin 0:9cf3812720c0 19 float map(float in, float inMin, float inMax, float outMin, float outMax);
AlanHuchin 0:9cf3812720c0 20 float multiMap(float val, float* _in, float* _out, int size);
AlanHuchin 0:9cf3812720c0 21 void error();
AlanHuchin 0:9cf3812720c0 22 void incremento();
AlanHuchin 0:9cf3812720c0 23
AlanHuchin 0:9cf3812720c0 24 float cnt = 0.0, grados = 0.0, adc = 0.0, e = 0.0, time_ = 0.0;
AlanHuchin 0:9cf3812720c0 25 int var = 0;
AlanHuchin 0:9cf3812720c0 26
AlanHuchin 0:9cf3812720c0 27 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};
AlanHuchin 0:9cf3812720c0 28 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};
AlanHuchin 0:9cf3812720c0 29 float z = 0.0;
AlanHuchin 0:9cf3812720c0 30
AlanHuchin 0:9cf3812720c0 31 int main() {
AlanHuchin 0:9cf3812720c0 32
AlanHuchin 0:9cf3812720c0 33 pc.baud(115200);
AlanHuchin 0:9cf3812720c0 34 pc.printf("-------------------------------------------------- \n\r");
AlanHuchin 0:9cf3812720c0 35
AlanHuchin 0:9cf3812720c0 36 esc1 = 0.0;
AlanHuchin 0:9cf3812720c0 37 wait(5.0);
AlanHuchin 0:9cf3812720c0 38
AlanHuchin 0:9cf3812720c0 39 pc.printf("Motores Activados\n\r" );
AlanHuchin 0:9cf3812720c0 40 pc.printf("Todo Listo...\n\r" );
AlanHuchin 0:9cf3812720c0 41 wait(1.0);
AlanHuchin 0:9cf3812720c0 42
AlanHuchin 0:9cf3812720c0 43 pb1.rise(&flip_pb1);
AlanHuchin 0:9cf3812720c0 44 pb2.rise(&flip_pb2);
AlanHuchin 0:9cf3812720c0 45 thread1.start(read_analog);
AlanHuchin 0:9cf3812720c0 46 thread2.start(error);
AlanHuchin 0:9cf3812720c0 47 thread3.start(incremento);
AlanHuchin 0:9cf3812720c0 48
AlanHuchin 0:9cf3812720c0 49 var = 1;
AlanHuchin 0:9cf3812720c0 50 t.start();
AlanHuchin 0:9cf3812720c0 51 while(1){
AlanHuchin 0:9cf3812720c0 52
AlanHuchin 0:9cf3812720c0 53 if(var == 1){
AlanHuchin 0:9cf3812720c0 54 //pc.printf("adc: %f, grad: %f, pwm: %f, e: %f, pwm_des:%f\n", adc, grados, cnt, e, z );
AlanHuchin 0:9cf3812720c0 55 pc.printf("%f, %f, %f \n", grados, t.read(),60.0);
AlanHuchin 0:9cf3812720c0 56 //pc.printf("%f, %f \n", grados, cnt);
AlanHuchin 0:9cf3812720c0 57 }
AlanHuchin 0:9cf3812720c0 58
AlanHuchin 0:9cf3812720c0 59 Thread::wait(20);
AlanHuchin 0:9cf3812720c0 60 }
AlanHuchin 0:9cf3812720c0 61
AlanHuchin 0:9cf3812720c0 62 }
AlanHuchin 0:9cf3812720c0 63
AlanHuchin 0:9cf3812720c0 64 void flip_pb1() {
AlanHuchin 0:9cf3812720c0 65
AlanHuchin 0:9cf3812720c0 66 wait_ms(50);
AlanHuchin 0:9cf3812720c0 67 cnt = cnt + 0.01;
AlanHuchin 0:9cf3812720c0 68 if (cnt > 1.0){cnt = 1.0;}
AlanHuchin 0:9cf3812720c0 69
AlanHuchin 0:9cf3812720c0 70 }
AlanHuchin 0:9cf3812720c0 71
AlanHuchin 0:9cf3812720c0 72
AlanHuchin 0:9cf3812720c0 73 void flip_pb2() {
AlanHuchin 0:9cf3812720c0 74
AlanHuchin 0:9cf3812720c0 75 wait_ms(50);
AlanHuchin 0:9cf3812720c0 76 cnt = cnt - 0.01;
AlanHuchin 0:9cf3812720c0 77 if (cnt < 0.0){cnt = 0.0;}
AlanHuchin 0:9cf3812720c0 78
AlanHuchin 0:9cf3812720c0 79 }
AlanHuchin 0:9cf3812720c0 80
AlanHuchin 0:9cf3812720c0 81
AlanHuchin 0:9cf3812720c0 82 void read_analog(){
AlanHuchin 0:9cf3812720c0 83 while(true){
AlanHuchin 0:9cf3812720c0 84
AlanHuchin 0:9cf3812720c0 85 adc = ain.read();
AlanHuchin 0:9cf3812720c0 86 grados = map(adc,0.45,0.98,90.0,0.0);
AlanHuchin 0:9cf3812720c0 87 e = ( 45.0 - grados);
AlanHuchin 0:9cf3812720c0 88
AlanHuchin 0:9cf3812720c0 89
AlanHuchin 0:9cf3812720c0 90 Thread::wait(10);
AlanHuchin 0:9cf3812720c0 91
AlanHuchin 0:9cf3812720c0 92 }
AlanHuchin 0:9cf3812720c0 93 }
AlanHuchin 0:9cf3812720c0 94
AlanHuchin 0:9cf3812720c0 95 void error(){
AlanHuchin 0:9cf3812720c0 96
AlanHuchin 0:9cf3812720c0 97 while(true){
AlanHuchin 0:9cf3812720c0 98 esc1 = z;
AlanHuchin 0:9cf3812720c0 99 Thread::wait(20);
AlanHuchin 0:9cf3812720c0 100
AlanHuchin 0:9cf3812720c0 101 }
AlanHuchin 0:9cf3812720c0 102 }
AlanHuchin 0:9cf3812720c0 103
AlanHuchin 0:9cf3812720c0 104 void incremento(){
AlanHuchin 0:9cf3812720c0 105
AlanHuchin 0:9cf3812720c0 106 while(true){
AlanHuchin 0:9cf3812720c0 107
AlanHuchin 0:9cf3812720c0 108 if(var == 1){
AlanHuchin 0:9cf3812720c0 109 z = multiMap(60, out, in, 473);
AlanHuchin 0:9cf3812720c0 110 if (t > 10.0){
AlanHuchin 0:9cf3812720c0 111 t.reset();
AlanHuchin 0:9cf3812720c0 112 t.stop();
AlanHuchin 0:9cf3812720c0 113 cnt = 0.0;
AlanHuchin 0:9cf3812720c0 114 var = 0;
AlanHuchin 0:9cf3812720c0 115 z=0.0;
AlanHuchin 0:9cf3812720c0 116 }
AlanHuchin 0:9cf3812720c0 117 }
AlanHuchin 0:9cf3812720c0 118
AlanHuchin 0:9cf3812720c0 119 Thread::wait(5);
AlanHuchin 0:9cf3812720c0 120 }
AlanHuchin 0:9cf3812720c0 121
AlanHuchin 0:9cf3812720c0 122 }
AlanHuchin 0:9cf3812720c0 123
AlanHuchin 0:9cf3812720c0 124 float map(float in, float inMin, float inMax, float outMin, float outMax) {
AlanHuchin 0:9cf3812720c0 125 // check it's within the range
AlanHuchin 0:9cf3812720c0 126 if (inMin<inMax) {
AlanHuchin 0:9cf3812720c0 127 if (in <= inMin)
AlanHuchin 0:9cf3812720c0 128 return outMin;
AlanHuchin 0:9cf3812720c0 129 if (in >= inMax)
AlanHuchin 0:9cf3812720c0 130 return outMax;
AlanHuchin 0:9cf3812720c0 131 } else { // cope with input range being backwards.
AlanHuchin 0:9cf3812720c0 132 if (in >= inMin)
AlanHuchin 0:9cf3812720c0 133 return outMin;
AlanHuchin 0:9cf3812720c0 134 if (in <= inMax)
AlanHuchin 0:9cf3812720c0 135 return outMax;
AlanHuchin 0:9cf3812720c0 136 }
AlanHuchin 0:9cf3812720c0 137 // calculate how far into the range we are
AlanHuchin 0:9cf3812720c0 138 float scale = (in-inMin)/(inMax-inMin);
AlanHuchin 0:9cf3812720c0 139 // calculate the output.
AlanHuchin 0:9cf3812720c0 140 return outMin + scale*(outMax-outMin);
AlanHuchin 0:9cf3812720c0 141 }
AlanHuchin 0:9cf3812720c0 142
AlanHuchin 0:9cf3812720c0 143 float multiMap(float val, float* _in, float* _out, int size)
AlanHuchin 0:9cf3812720c0 144 {
AlanHuchin 0:9cf3812720c0 145 // take care the value is within range
AlanHuchin 0:9cf3812720c0 146 // val = constrain(val, _in[0], _in[size-1]);
AlanHuchin 0:9cf3812720c0 147 if (val <= _in[0]) return _out[0];
AlanHuchin 0:9cf3812720c0 148 if (val >= _in[size-1]) return _out[size-1];
AlanHuchin 0:9cf3812720c0 149
AlanHuchin 0:9cf3812720c0 150 // search right interval
AlanHuchin 0:9cf3812720c0 151 int pos = 1; // _in[0] allready tested
AlanHuchin 0:9cf3812720c0 152 while(val > _in[pos]){
AlanHuchin 0:9cf3812720c0 153 pos++;
AlanHuchin 0:9cf3812720c0 154 }
AlanHuchin 0:9cf3812720c0 155
AlanHuchin 0:9cf3812720c0 156 // this will handle all exact "points" in the _in array
AlanHuchin 0:9cf3812720c0 157 if (val == _in[pos]) return _out[pos];
AlanHuchin 0:9cf3812720c0 158
AlanHuchin 0:9cf3812720c0 159 // interpolate in the right segment for the rest
AlanHuchin 0:9cf3812720c0 160 return (val - _in[pos-1]) * (_out[pos] - _out[pos-1]) / (_in[pos] - _in[pos-1]) + _out[pos-1];
AlanHuchin 0:9cf3812720c0 161 }