Robot

Dependencies:   Servo mbed wav_header wave_player TextLCD RemoteIR

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 #include "Servo.h"
00003 #include  "Speaker.h"
00004 #include "ReceiverIR.h"
00005 #define TEST_LOOP_BACK  0
00006 #define DEBOUNCEDELAY 500
00007 //pines libres p15,p16,p17,p19,p30, "p26 socket IF-r" 
00008 Serial Bt(p9,p10);
00009 Serial Wf(p28,p27);
00010 Serial Pc(USBTX, USBRX);
00011 ReceiverIR IR_Receiver(p15);
00012 PwmOut IR_Transmitter(p26); 
00013 DigitalOut Lights(p19);
00014 DigitalOut Motor2A(p7);
00015 DigitalOut Motor2B(p6);
00016 DigitalOut Motor4A(p13);
00017 DigitalOut Motor4B(p14);
00018 DigitalOut Motor3A(p8);
00019 DigitalOut Motor3B(p5);
00020 DigitalOut Motor1A(p12);
00021 DigitalOut Motor1B(p11);
00022 Servo myservo1(p21);
00023 Servo myservo2(p22);
00024 Servo myservo3(p23);
00025 Servo myservo4(p24);
00026 Speaker mySpeaker(p18);
00027 char K,F;
00028 int u,l=0;
00029 float max = 0.0006;
00030 float min = -0.0005;
00031 float position1 = 0.0,position2 = 0.0;
00032     int receive(RemoteIR::Format *format, uint8_t *buf, int bufsiz, int timeout = 100)
00033  {
00034     int cnt = 0;
00035     while (IR_Receiver.getState() != ReceiverIR::Received) {cnt++;if (timeout < cnt) {return -1;}}
00036     return IR_Receiver.getData(format, buf, bufsiz * 8);
00037  }
00038 /////////IR Transmitter
00039 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,};
00040 
00041 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
00042 
00043 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,}; 
00044 
00045 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,};
00046                 
00047 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,};
00048                
00049 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,};
00050                  
00051 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,};
00052                 
00053 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,};
00054                                           
00055 void PowerOff()
00056 {
00057     IR_Transmitter.period_us( 26 );int iCountHighLow = sizeof( Power_On_Off ) / sizeof( Power_On_Off[0] );
00058     for( int iIndexHighLow = 0; iIndexHighLow < iCountHighLow; iIndexHighLow++ ) {IR_Transmitter.write( 0.5 * (1 - (iIndexHighLow % 2)) ); wait_us( Power_On_Off[iIndexHighLow] );}   
00059 }
00060 void VolumeUp()
00061 {
00062     IR_Transmitter.period_us( 26 );int iCountHighLow = sizeof( Volume_Up ) / sizeof( Volume_Up[0] );
00063     for( int iIndexHighLow = 0; iIndexHighLow < iCountHighLow; iIndexHighLow++ ) {IR_Transmitter.write( 0.5 * (1 - (iIndexHighLow % 2)) );wait_us( Volume_Up[iIndexHighLow] );}
00064 }
00065 void VolumeDown()
00066 {
00067     IR_Transmitter.period_us( 26 );int iCountHighLow = sizeof( Volume_Down ) / sizeof( Volume_Down[0] );
00068     for( int iIndexHighLow = 0; iIndexHighLow < iCountHighLow; iIndexHighLow++ ) {IR_Transmitter.write( 0.5 * (1 - (iIndexHighLow % 2)) );wait_us( Volume_Down[iIndexHighLow] );}
00069 }
00070 void One()
00071 {
00072      IR_Transmitter.period_us( 26 ); int iCountHighLow = sizeof( One_B ) / sizeof( One_B[0] );
00073      for( int iIndexHighLow = 0; iIndexHighLow < iCountHighLow; iIndexHighLow++ ) {IR_Transmitter.write( 0.5 * (1 - (iIndexHighLow % 2)) );wait_us( One_B[iIndexHighLow] );}
00074     
00075 }
00076 void Two()
00077 {
00078     IR_Transmitter.period_us( 26 ); int iCountHighLow = sizeof( Two_B ) / sizeof( Two_B[0] );
00079     for( int iIndexHighLow = 0; iIndexHighLow < iCountHighLow; iIndexHighLow++ ) {IR_Transmitter.write( 0.5 * (1 - (iIndexHighLow % 2)) );wait_us( Two_B[iIndexHighLow] );}
00080 }
00081 void Three()
00082 {
00083     IR_Transmitter.period_us( 26 );int iCountHighLow = sizeof( Three_B ) / sizeof( Three_B[0] );
00084     for( int iIndexHighLow = 0; iIndexHighLow < iCountHighLow; iIndexHighLow++ ) {IR_Transmitter.write( 0.5 * (1 - (iIndexHighLow % 2)) );wait_us( Three_B[iIndexHighLow] );}
00085 }
00086 void Four()
00087 {
00088     IR_Transmitter.period_us( 26 );int iCountHighLow = sizeof( Four_B ) / sizeof( Four_B[0] );
00089     for( int iIndexHighLow = 0; iIndexHighLow < iCountHighLow; iIndexHighLow++ ) {IR_Transmitter.write( 0.5 * (1 - (iIndexHighLow % 2)) );wait_us( Four_B[iIndexHighLow] );}
00090 }  
00091 void Five()
00092 {
00093     IR_Transmitter.period_us( 26 );int iCountHighLow = sizeof( Five_B ) / sizeof( Five_B[0] );
00094     for( int iIndexHighLow = 0; iIndexHighLow < iCountHighLow; iIndexHighLow++ ) {IR_Transmitter.write( 0.5 * (1 - (iIndexHighLow % 2)) );wait_us( Five_B[iIndexHighLow] );}
00095 }
00096 
00097 int main() {
00098  Timer timer;
00099  timer.start();
00100  int D1A,D1B,D2A,D2B,D3A,D3B,D4A,D4B,x4,y4,x2,y2;
00101  int iMilliSec_prev = timer.read_ms();
00102      myservo1.calibrate_max(max, 90); myservo1.calibrate_min(min, 0);
00103      myservo2.calibrate_max(max, 90); myservo2.calibrate_min(min, 0);
00104      myservo3.calibrate_max(max, 90); myservo3.calibrate_min(min, 0);
00105      myservo4.calibrate_max(max, 90); myservo4.calibrate_min(min, 0);
00106      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;
00107   while(1) {
00108     Motor1A=0; Motor1B=0; Motor2A=0; Motor2B=0;  Motor3A=0; Motor3B=0; Motor4A=0; Motor4B=0; // Motor Stop
00109     if (Bt.readable()){K=Bt.getc();}
00110     if (Wf.readable()){K=Wf.getc();}
00111     if (Pc.readable()){K=Pc.getc();}
00112 ///////////   Infrared Transmitter   //////////////    
00113     if (K=='1'){ One();        IR_Transmitter=0;IR_Transmitter.period(0.020);K=0;}
00114     if (K=='2'){ Two();        IR_Transmitter=0;IR_Transmitter.period(0.020);K=0;}
00115     if (K=='3'){ Three();      IR_Transmitter=0;IR_Transmitter.period(0.020);K=0;}
00116     if (K=='4'){ Four();       IR_Transmitter=0;IR_Transmitter.period(0.020);K=0;}
00117     if (K=='5'){ Five();       IR_Transmitter=0;IR_Transmitter.period(0.020);K=0;}
00118     if (K=='p'){ PowerOff();   IR_Transmitter=0;IR_Transmitter.period(0.020);K=0;}
00119     if (K=='u'){ VolumeUp();   IR_Transmitter=0;IR_Transmitter.period(0.020);K=0;}
00120     if (K=='d'){ VolumeDown(); IR_Transmitter=0;IR_Transmitter.period(0.020);K=0;}
00121 ///////////   Servo   //////////////     
00122     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;}
00123     if (K=='8'){ myservo1 =0.5;  myservo2 = 0.5 ; myservo3= 0.5; myservo4 =0.5;}
00124     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;}
00125     if (K=='6'){ myservo1 =0; myservo2 = 0; myservo3= 0; myservo4 =0;K=0;}
00126 ///////////   Lights  //////////////     
00127     if (K=='l'){ if(l==1){Lights=0;l=0;K=0;}else {Lights=1;l=1;K=0;}}
00128 ///////////   Wifly Config   //////////////     
00129     if (K=='c'){ while(1){while(Bt.readable())Wf.putc(Bt.getc());while(Wf.readable())Bt.putc(Wf.getc());}}
00130 ///////////   Claxon   ////////////// 
00131     if (K=='k'){  mySpeaker.PlayNote(969.0, 0.5, 1.0);mySpeaker.PlayNote(800.0, 0.5, 1.0);K=0;}
00132 ///////////   Motores   ////////////// 
00133     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
00134     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
00135     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
00136     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
00137    
00138     Wf.printf("\n\r");
00139    Pc.printf("%c",K);
00140 ///////////   Infrared Receiver   ////////////// 
00141      uint8_t buf1[32];
00142      int bitlength1; 
00143      RemoteIR::Format format;memset(buf1, 0x00, sizeof(buf1));bitlength1 = receive(&format, buf1, sizeof(buf1));
00144      if (bitlength1 < 0) {continue;}
00145      for (int i = 0; i <4; i++) 
00146      {for (int i = 0; i <1; i++) 
00147              { u=buf1[i];
00148                if(u==244){K=0x41;} // Forward
00149                if(u==245){K=0x42;}// Backward
00150                if(u==179){K=0x43;}// Right
00151                if(u==180){K=0x44;}//Left
00152                if(u==229){K=0x0D;}//Horn
00153                if(u==133){K='6';}// Servos Position 0 movement
00154                if(u==134){K='7';}// Servos left movement
00155                if(u==135){K='8';}// Servos Star movement
00156                if(u==136){K='9';}// Servos Right movement
00157                if(u==149){K='o';}// Robot on
00158                if(u==224){K='l';}// Lights
00159                if(u==139){K='p';}// Tv on/off
00160              }
00161       }
00162        Bt.printf("%i",u); Bt.printf("\n\r");
00163     }
00164 }
00165 
00166 
00167