J Daniel Martinez C / Mbed 2 deprecated OMNi-Robot_no_borrar

Robot

Dependencies:   Servo mbed wav_header wave_player TextLCD RemoteIR

main.cpp@0:39f6dbb94f99, 2014-06-27 (annotated)

Committer:
dan_cuspi
Date:
Fri Jun 27 23:39:12 2014 +0000
Revision:
0:39f6dbb94f99
hola

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dan_cuspi 0:39f6dbb94f99 1 #include "mbed.h"
dan_cuspi 0:39f6dbb94f99 2 #include "Servo.h"
dan_cuspi 0:39f6dbb94f99 3 #include "Speaker.h"
dan_cuspi 0:39f6dbb94f99 4 #include "ReceiverIR.h"
dan_cuspi 0:39f6dbb94f99 5 #define TEST_LOOP_BACK 0
dan_cuspi 0:39f6dbb94f99 6 #define DEBOUNCEDELAY 500
dan_cuspi 0:39f6dbb94f99 7 //pines libres p15,p16,p17,p19,p30, "p26 socket IF-r"
dan_cuspi 0:39f6dbb94f99 8 Serial Bt(p9,p10);
dan_cuspi 0:39f6dbb94f99 9 Serial Wf(p28,p27);
dan_cuspi 0:39f6dbb94f99 10 Serial Pc(USBTX, USBRX);
dan_cuspi 0:39f6dbb94f99 11 ReceiverIR IR_Receiver(p15);
dan_cuspi 0:39f6dbb94f99 12 PwmOut IR_Transmitter(p26);
dan_cuspi 0:39f6dbb94f99 13 DigitalOut Lights(p19);
dan_cuspi 0:39f6dbb94f99 14 DigitalOut Motor2A(p7);
dan_cuspi 0:39f6dbb94f99 15 DigitalOut Motor2B(p6);
dan_cuspi 0:39f6dbb94f99 16 DigitalOut Motor4A(p13);
dan_cuspi 0:39f6dbb94f99 17 DigitalOut Motor4B(p14);
dan_cuspi 0:39f6dbb94f99 18 DigitalOut Motor3A(p8);
dan_cuspi 0:39f6dbb94f99 19 DigitalOut Motor3B(p5);
dan_cuspi 0:39f6dbb94f99 20 DigitalOut Motor1A(p12);
dan_cuspi 0:39f6dbb94f99 21 DigitalOut Motor1B(p11);
dan_cuspi 0:39f6dbb94f99 22 Servo myservo1(p21);
dan_cuspi 0:39f6dbb94f99 23 Servo myservo2(p22);
dan_cuspi 0:39f6dbb94f99 24 Servo myservo3(p23);
dan_cuspi 0:39f6dbb94f99 25 Servo myservo4(p24);
dan_cuspi 0:39f6dbb94f99 26 Speaker mySpeaker(p18);
dan_cuspi 0:39f6dbb94f99 27 char K,F;
dan_cuspi 0:39f6dbb94f99 28 int u,l=0;
dan_cuspi 0:39f6dbb94f99 29 float max = 0.0006;
dan_cuspi 0:39f6dbb94f99 30 float min = -0.0005;
dan_cuspi 0:39f6dbb94f99 31 float position1 = 0.0,position2 = 0.0;
dan_cuspi 0:39f6dbb94f99 32 int receive(RemoteIR::Format *format, uint8_t *buf, int bufsiz, int timeout = 100)
dan_cuspi 0:39f6dbb94f99 33 {
dan_cuspi 0:39f6dbb94f99 34 int cnt = 0;
dan_cuspi 0:39f6dbb94f99 35 while (IR_Receiver.getState() != ReceiverIR::Received) {cnt++;if (timeout < cnt) {return -1;}}
dan_cuspi 0:39f6dbb94f99 36 return IR_Receiver.getData(format, buf, bufsiz * 8);
dan_cuspi 0:39f6dbb94f99 37 }
dan_cuspi 0:39f6dbb94f99 38 /////////IR Transmitter
dan_cuspi 0:39f6dbb94f99 39 short Power_On_Off[] = { 2460, 543, 1290, 514, 692, 507, 1289, 514, 692, 507, 1290, 514, 691, 507, 690, 508, 1289, 516, 717, 481,690, 508, 689, 508, 692, 25634,};
dan_cuspi 0:39f6dbb94f99 40
dan_cuspi 0:39f6dbb94f99 41 short Volume_Up[] = { 2500,440,700,480,1280,460,680,480,700,460,1300,460,700,460,700,460,1280,460,700,480,700,460,680,480,700,25620,}; //Volume Up
dan_cuspi 0:39f6dbb94f99 42
dan_cuspi 0:39f6dbb94f99 43 short Volume_Down[]= { 2480,480, 1260,500, 1260,480, 680,500, 660,500, 1240,500, 680,480, 680,460, 1280,500, 660,500, 680,480, 680,480, 680,25040,};
dan_cuspi 0:39f6dbb94f99 44
dan_cuspi 0:39f6dbb94f99 45 short One_B[]= { 2480,460,700,460,700,460,720,440,720,460,700,460,700,460,720,440,1320,440,700,460,700,460,700,460,700,26800,};
dan_cuspi 0:39f6dbb94f99 46
dan_cuspi 0:39f6dbb94f99 47 short Two_B[]= { 2500,440,1300,460,680,480,680,480,680,480,680,480,680,480,700,480,1280,460,680,480,680,480,700,460,700,26220,};
dan_cuspi 0:39f6dbb94f99 48
dan_cuspi 0:39f6dbb94f99 49 short Three_B[]= { 2500,440,680,480,1300,440,700,480,680,480,680,480,680,480,680,500,1280,460,680,480,680,480,680,480,680,26240,};
dan_cuspi 0:39f6dbb94f99 50
dan_cuspi 0:39f6dbb94f99 51 short Four_B[]= { 2520,420,1300,460,1300,440,700,480,680,480,680,480,680,480,680,480,1300,460,680,480,680,480,680,480,680,25640,};
dan_cuspi 0:39f6dbb94f99 52
dan_cuspi 0:39f6dbb94f99 53 short Five_B[]= { 2500,460,680,480,680,480,1300,440,680,500,680,480,680,480,680,480,1300,440,700,480,680,480,680,480,680,26220,};
dan_cuspi 0:39f6dbb94f99 54
dan_cuspi 0:39f6dbb94f99 55 void PowerOff()
dan_cuspi 0:39f6dbb94f99 56 {
dan_cuspi 0:39f6dbb94f99 57 IR_Transmitter.period_us( 26 );int iCountHighLow = sizeof( Power_On_Off ) / sizeof( Power_On_Off[0] );
dan_cuspi 0:39f6dbb94f99 58 for( int iIndexHighLow = 0; iIndexHighLow < iCountHighLow; iIndexHighLow++ ) {IR_Transmitter.write( 0.5 * (1 - (iIndexHighLow % 2)) ); wait_us( Power_On_Off[iIndexHighLow] );}
dan_cuspi 0:39f6dbb94f99 59 }
dan_cuspi 0:39f6dbb94f99 60 void VolumeUp()
dan_cuspi 0:39f6dbb94f99 61 {
dan_cuspi 0:39f6dbb94f99 62 IR_Transmitter.period_us( 26 );int iCountHighLow = sizeof( Volume_Up ) / sizeof( Volume_Up[0] );
dan_cuspi 0:39f6dbb94f99 63 for( int iIndexHighLow = 0; iIndexHighLow < iCountHighLow; iIndexHighLow++ ) {IR_Transmitter.write( 0.5 * (1 - (iIndexHighLow % 2)) );wait_us( Volume_Up[iIndexHighLow] );}
dan_cuspi 0:39f6dbb94f99 64 }
dan_cuspi 0:39f6dbb94f99 65 void VolumeDown()
dan_cuspi 0:39f6dbb94f99 66 {
dan_cuspi 0:39f6dbb94f99 67 IR_Transmitter.period_us( 26 );int iCountHighLow = sizeof( Volume_Down ) / sizeof( Volume_Down[0] );
dan_cuspi 0:39f6dbb94f99 68 for( int iIndexHighLow = 0; iIndexHighLow < iCountHighLow; iIndexHighLow++ ) {IR_Transmitter.write( 0.5 * (1 - (iIndexHighLow % 2)) );wait_us( Volume_Down[iIndexHighLow] );}
dan_cuspi 0:39f6dbb94f99 69 }
dan_cuspi 0:39f6dbb94f99 70 void One()
dan_cuspi 0:39f6dbb94f99 71 {
dan_cuspi 0:39f6dbb94f99 72 IR_Transmitter.period_us( 26 ); int iCountHighLow = sizeof( One_B ) / sizeof( One_B[0] );
dan_cuspi 0:39f6dbb94f99 73 for( int iIndexHighLow = 0; iIndexHighLow < iCountHighLow; iIndexHighLow++ ) {IR_Transmitter.write( 0.5 * (1 - (iIndexHighLow % 2)) );wait_us( One_B[iIndexHighLow] );}
dan_cuspi 0:39f6dbb94f99 74
dan_cuspi 0:39f6dbb94f99 75 }
dan_cuspi 0:39f6dbb94f99 76 void Two()
dan_cuspi 0:39f6dbb94f99 77 {
dan_cuspi 0:39f6dbb94f99 78 IR_Transmitter.period_us( 26 ); int iCountHighLow = sizeof( Two_B ) / sizeof( Two_B[0] );
dan_cuspi 0:39f6dbb94f99 79 for( int iIndexHighLow = 0; iIndexHighLow < iCountHighLow; iIndexHighLow++ ) {IR_Transmitter.write( 0.5 * (1 - (iIndexHighLow % 2)) );wait_us( Two_B[iIndexHighLow] );}
dan_cuspi 0:39f6dbb94f99 80 }
dan_cuspi 0:39f6dbb94f99 81 void Three()
dan_cuspi 0:39f6dbb94f99 82 {
dan_cuspi 0:39f6dbb94f99 83 IR_Transmitter.period_us( 26 );int iCountHighLow = sizeof( Three_B ) / sizeof( Three_B[0] );
dan_cuspi 0:39f6dbb94f99 84 for( int iIndexHighLow = 0; iIndexHighLow < iCountHighLow; iIndexHighLow++ ) {IR_Transmitter.write( 0.5 * (1 - (iIndexHighLow % 2)) );wait_us( Three_B[iIndexHighLow] );}
dan_cuspi 0:39f6dbb94f99 85 }
dan_cuspi 0:39f6dbb94f99 86 void Four()
dan_cuspi 0:39f6dbb94f99 87 {
dan_cuspi 0:39f6dbb94f99 88 IR_Transmitter.period_us( 26 );int iCountHighLow = sizeof( Four_B ) / sizeof( Four_B[0] );
dan_cuspi 0:39f6dbb94f99 89 for( int iIndexHighLow = 0; iIndexHighLow < iCountHighLow; iIndexHighLow++ ) {IR_Transmitter.write( 0.5 * (1 - (iIndexHighLow % 2)) );wait_us( Four_B[iIndexHighLow] );}
dan_cuspi 0:39f6dbb94f99 90 }
dan_cuspi 0:39f6dbb94f99 91 void Five()
dan_cuspi 0:39f6dbb94f99 92 {
dan_cuspi 0:39f6dbb94f99 93 IR_Transmitter.period_us( 26 );int iCountHighLow = sizeof( Five_B ) / sizeof( Five_B[0] );
dan_cuspi 0:39f6dbb94f99 94 for( int iIndexHighLow = 0; iIndexHighLow < iCountHighLow; iIndexHighLow++ ) {IR_Transmitter.write( 0.5 * (1 - (iIndexHighLow % 2)) );wait_us( Five_B[iIndexHighLow] );}
dan_cuspi 0:39f6dbb94f99 95 }
dan_cuspi 0:39f6dbb94f99 96
dan_cuspi 0:39f6dbb94f99 97 int main() {
dan_cuspi 0:39f6dbb94f99 98 Timer timer;
dan_cuspi 0:39f6dbb94f99 99 timer.start();
dan_cuspi 0:39f6dbb94f99 100 int D1A,D1B,D2A,D2B,D3A,D3B,D4A,D4B,x4,y4,x2,y2;
dan_cuspi 0:39f6dbb94f99 101 int iMilliSec_prev = timer.read_ms();
dan_cuspi 0:39f6dbb94f99 102 myservo1.calibrate_max(max, 90); myservo1.calibrate_min(min, 0);
dan_cuspi 0:39f6dbb94f99 103 myservo2.calibrate_max(max, 90); myservo2.calibrate_min(min, 0);
dan_cuspi 0:39f6dbb94f99 104 myservo3.calibrate_max(max, 90); myservo3.calibrate_min(min, 0);
dan_cuspi 0:39f6dbb94f99 105 myservo4.calibrate_max(max, 90); myservo4.calibrate_min(min, 0);
dan_cuspi 0:39f6dbb94f99 106 myservo1 =0.95;myservo2 = 0.0; myservo3= 0.90; myservo4 =0.0;D1A=1,D1B=0,D2A=1,D2B=0,D3A=1,D3B=0,D4A=1,D4B=0 ;x2=D2A; y2=D2B; x4=D4A; y4=D4B;
dan_cuspi 0:39f6dbb94f99 107 while(1) {
dan_cuspi 0:39f6dbb94f99 108 Motor1A=0; Motor1B=0; Motor2A=0; Motor2B=0; Motor3A=0; Motor3B=0; Motor4A=0; Motor4B=0; // Motor Stop
dan_cuspi 0:39f6dbb94f99 109 if (Bt.readable()){K=Bt.getc();}
dan_cuspi 0:39f6dbb94f99 110 if (Wf.readable()){K=Wf.getc();}
dan_cuspi 0:39f6dbb94f99 111 if (Pc.readable()){K=Pc.getc();}
dan_cuspi 0:39f6dbb94f99 112 /////////// Infrared Transmitter //////////////
dan_cuspi 0:39f6dbb94f99 113 if (K=='1'){ One(); IR_Transmitter=0;IR_Transmitter.period(0.020);K=0;}
dan_cuspi 0:39f6dbb94f99 114 if (K=='2'){ Two(); IR_Transmitter=0;IR_Transmitter.period(0.020);K=0;}
dan_cuspi 0:39f6dbb94f99 115 if (K=='3'){ Three(); IR_Transmitter=0;IR_Transmitter.period(0.020);K=0;}
dan_cuspi 0:39f6dbb94f99 116 if (K=='4'){ Four(); IR_Transmitter=0;IR_Transmitter.period(0.020);K=0;}
dan_cuspi 0:39f6dbb94f99 117 if (K=='5'){ Five(); IR_Transmitter=0;IR_Transmitter.period(0.020);K=0;}
dan_cuspi 0:39f6dbb94f99 118 if (K=='p'){ PowerOff(); IR_Transmitter=0;IR_Transmitter.period(0.020);K=0;}
dan_cuspi 0:39f6dbb94f99 119 if (K=='u'){ VolumeUp(); IR_Transmitter=0;IR_Transmitter.period(0.020);K=0;}
dan_cuspi 0:39f6dbb94f99 120 if (K=='d'){ VolumeDown(); IR_Transmitter=0;IR_Transmitter.period(0.020);K=0;}
dan_cuspi 0:39f6dbb94f99 121 /////////// Servo //////////////
dan_cuspi 0:39f6dbb94f99 122 if (K=='7'){ myservo1 =0.0; myservo2 = .90; myservo3= 0.0; myservo4 =.90; D1A=1,D1B=0,D2A=0,D2B=1,D3A=1,D3B=0,D4A=0,D4B=1; x2=D2B; y2=D2A; x4=D4B; y4=D4A;}
dan_cuspi 0:39f6dbb94f99 123 if (K=='8'){ myservo1 =0.5; myservo2 = 0.5 ; myservo3= 0.5; myservo4 =0.5;}
dan_cuspi 0:39f6dbb94f99 124 if (K=='9'){ myservo1 =.90; myservo2 = 0.0; myservo3= .90; myservo4 =0; D1A=1,D1B=0,D2A=1,D2B=0,D3A=1,D3B=0,D4A=1,D4B=0 ;x2=D2A; y2=D2B; x4=D4A; y4=D4B;}
dan_cuspi 0:39f6dbb94f99 125 if (K=='6'){ myservo1 =0; myservo2 = 0; myservo3= 0; myservo4 =0;K=0;}
dan_cuspi 0:39f6dbb94f99 126 /////////// Lights //////////////
dan_cuspi 0:39f6dbb94f99 127 if (K=='l'){ if(l==1){Lights=0;l=0;K=0;}else {Lights=1;l=1;K=0;}}
dan_cuspi 0:39f6dbb94f99 128 /////////// Wifly Config //////////////
dan_cuspi 0:39f6dbb94f99 129 if (K=='c'){ while(1){while(Bt.readable())Wf.putc(Bt.getc());while(Wf.readable())Bt.putc(Wf.getc());}}
dan_cuspi 0:39f6dbb94f99 130 /////////// Claxon //////////////
dan_cuspi 0:39f6dbb94f99 131 if (K=='k'){ mySpeaker.PlayNote(969.0, 0.5, 1.0);mySpeaker.PlayNote(800.0, 0.5, 1.0);K=0;}
dan_cuspi 0:39f6dbb94f99 132 /////////// Motores //////////////
dan_cuspi 0:39f6dbb94f99 133 if (K==0x41||K=='t'){Motor1A=D1A; Motor1B=D1B; Motor2A=D2A; Motor2B=D2B; Motor3A=D3A; Motor3B=D3B; Motor4A=D4A; Motor4B=D4B; wait(0.1); K=0;} //forward
dan_cuspi 0:39f6dbb94f99 134 if (K==0x42||K=='g'){Motor1A=D1B; Motor1B=D1A; Motor2A=D2B; Motor2B=D2A; Motor3A=D3B; Motor3B=D3A; Motor4A=D4B; Motor4B=D4A; wait(0.1); K=0;} //backward
dan_cuspi 0:39f6dbb94f99 135 if (K==0x43||K=='h'){Motor1A=D1B; Motor1B=D1A; Motor3A=D3A; Motor3B=D3B; Motor2A=x2; Motor2B=y2; Motor4A=y4; Motor4B=x4; wait(0.016); K=0;} //right
dan_cuspi 0:39f6dbb94f99 136 if (K==0x44||K=='f'){Motor1A=D1A; Motor1B=D1B; Motor3A=D3B; Motor3B=D3A; Motor2A=y2; Motor2B=x2; Motor4A=x4; Motor4B=y4; wait(0.016); K=0;} //left
dan_cuspi 0:39f6dbb94f99 137
dan_cuspi 0:39f6dbb94f99 138 Wf.printf("\n\r");
dan_cuspi 0:39f6dbb94f99 139 Pc.printf("%c",K);
dan_cuspi 0:39f6dbb94f99 140 /////////// Infrared Receiver //////////////
dan_cuspi 0:39f6dbb94f99 141 uint8_t buf1[32];
dan_cuspi 0:39f6dbb94f99 142 int bitlength1;
dan_cuspi 0:39f6dbb94f99 143 RemoteIR::Format format;memset(buf1, 0x00, sizeof(buf1));bitlength1 = receive(&format, buf1, sizeof(buf1));
dan_cuspi 0:39f6dbb94f99 144 if (bitlength1 < 0) {continue;}
dan_cuspi 0:39f6dbb94f99 145 for (int i = 0; i <4; i++)
dan_cuspi 0:39f6dbb94f99 146 {for (int i = 0; i <1; i++)
dan_cuspi 0:39f6dbb94f99 147 { u=buf1[i];
dan_cuspi 0:39f6dbb94f99 148 if(u==244){K=0x41;} // Forward
dan_cuspi 0:39f6dbb94f99 149 if(u==245){K=0x42;}// Backward
dan_cuspi 0:39f6dbb94f99 150 if(u==179){K=0x43;}// Right
dan_cuspi 0:39f6dbb94f99 151 if(u==180){K=0x44;}//Left
dan_cuspi 0:39f6dbb94f99 152 if(u==229){K=0x0D;}//Horn
dan_cuspi 0:39f6dbb94f99 153 if(u==133){K='6';}// Servos Position 0 movement
dan_cuspi 0:39f6dbb94f99 154 if(u==134){K='7';}// Servos left movement
dan_cuspi 0:39f6dbb94f99 155 if(u==135){K='8';}// Servos Star movement
dan_cuspi 0:39f6dbb94f99 156 if(u==136){K='9';}// Servos Right movement
dan_cuspi 0:39f6dbb94f99 157 if(u==149){K='o';}// Robot on
dan_cuspi 0:39f6dbb94f99 158 if(u==224){K='l';}// Lights
dan_cuspi 0:39f6dbb94f99 159 if(u==139){K='p';}// Tv on/off
dan_cuspi 0:39f6dbb94f99 160 }
dan_cuspi 0:39f6dbb94f99 161 }
dan_cuspi 0:39f6dbb94f99 162 Bt.printf("%i",u); Bt.printf("\n\r");
dan_cuspi 0:39f6dbb94f99 163 }
dan_cuspi 0:39f6dbb94f99 164 }
dan_cuspi 0:39f6dbb94f99 165
dan_cuspi 0:39f6dbb94f99 166
dan_cuspi 0:39f6dbb94f99 167