TP_presa

Dependencies:   SRF08 Servo mbed

main.cpp

Committer:
Remi95
Date:
2017-05-31
Revision:
0:99cb324a2329
Child:
1:5830b7d6a985

File content as of revision 0:99cb324a2329:

#include "mbed.h"

char Adress = 128;
                    //Address: 130
                    //Command : 0
                    //Data: 64
                    //Checksum: 66
                    //Checksum Putc((address + 0 + speed) & 127);

Serial sbt(p9, p10); // tx, rx

DigitalOut myled1(LED1);
DigitalOut myled2(LED2);
DigitalOut myled3(LED3);
DigitalOut myled4(LED4);
DigitalOut S2 (p11);


void Led (bool l1,bool l2,bool l3,bool l4){  //Fonction pour aider au debug avec les led
  
    myled1=l1;
    myled2=l2;
    myled3=l3;
    myled4=l4;    
}

void forward(char Vit) {
     sbt.putc(Adress);
     sbt.putc(0); 
     sbt.putc(Vit);  
     sbt.putc((Adress+0+Vit) & 127);
     
     sbt.putc(Adress);
     sbt.putc(4); 
     sbt.putc(Vit);  
     sbt.putc((Adress+4+Vit) & 127);
}
void avancer(void) {
     Vit=50;
     sbt.putc(Adress);
     sbt.putc(0); 
     sbt.putc(Vit);  
     sbt.putc((Adress+0+Vit) & 127);
     
     sbt.putc(Adress);
     sbt.putc(4); 
     sbt.putc(Vit);  
     sbt.putc((Adress+4+Vit) & 127);
}
void reculer(void) {
     Vit=50;
     sbt.putc(Adress);
     sbt.putc(1); 
     sbt.putc(Vit);  
     sbt.putc((Adress+0+Vit) & 127);
     
     sbt.putc(Adress);
     sbt.putc(5); 
     sbt.putc(Vit);  
     sbt.putc((Adress+4+Vit) & 127);
}


int main (){
    Led (0,1,0,1);
    S2=0;
    wait(1);
    forward(50);
}