Mathias Lyngklip / Mbed 2 deprecated RoboticHackathon

hej

Dependencies:   dillerdasker SG90 hack_motor mbed

main.cpp@0:47165eb4e54d, 2014-04-05 (annotated)

Committer:
iLyngklip
Date:
Sat Apr 05 07:59:04 2014 +0000
Revision:
0:47165eb4e54d
hej

Who changed what in which revision?

UserRevisionLine numberNew contents of line
iLyngklip 0:47165eb4e54d 1 #include "mbed.h"
iLyngklip 0:47165eb4e54d 2 #include "HCSR04.h"
iLyngklip 0:47165eb4e54d 3 #include "SG90.h"
iLyngklip 0:47165eb4e54d 4 #include "PwmOut.h"
iLyngklip 0:47165eb4e54d 5
iLyngklip 0:47165eb4e54d 6
iLyngklip 0:47165eb4e54d 7 Serial pc(USBTX, USBRX);
iLyngklip 0:47165eb4e54d 8 servo myservo;
iLyngklip 0:47165eb4e54d 9
iLyngklip 0:47165eb4e54d 10 int i = 0;
iLyngklip 0:47165eb4e54d 11 int G0 = 0;
iLyngklip 0:47165eb4e54d 12 int G15 = 0;
iLyngklip 0:47165eb4e54d 13 int G30 = 0;
iLyngklip 0:47165eb4e54d 14 int G45 = 0;
iLyngklip 0:47165eb4e54d 15 int G60 = 0;
iLyngklip 0:47165eb4e54d 16 int G75 = 0;
iLyngklip 0:47165eb4e54d 17 int G90 = 0;
iLyngklip 0:47165eb4e54d 18 int G105 = 0;
iLyngklip 0:47165eb4e54d 19 int G120 = 0;
iLyngklip 0:47165eb4e54d 20 int G135 = 0;
iLyngklip 0:47165eb4e54d 21 int G150 = 0;
iLyngklip 0:47165eb4e54d 22 int G165 = 0;
iLyngklip 0:47165eb4e54d 23 int G180 = 0;
iLyngklip 0:47165eb4e54d 24
iLyngklip 0:47165eb4e54d 25 float m = 0.5;
iLyngklip 0:47165eb4e54d 26
iLyngklip 0:47165eb4e54d 27 PwmOut left(PTE23 );
iLyngklip 0:47165eb4e54d 28 PwmOut right(PTE22 );
iLyngklip 0:47165eb4e54d 29 int delay = 200; // Delay mellem målingerne
iLyngklip 0:47165eb4e54d 30 double B = 0; // Vinklen B Tegning 1
iLyngklip 0:47165eb4e54d 31 int A = 179; // Vinkel mellem de to målinger
iLyngklip 0:47165eb4e54d 32
iLyngklip 0:47165eb4e54d 33
iLyngklip 0:47165eb4e54d 34
iLyngklip 0:47165eb4e54d 35 int main(){
iLyngklip 0:47165eb4e54d 36 pc.baud(9600);
iLyngklip 0:47165eb4e54d 37 HCSR04 sensor(PTA13, PTD5);
iLyngklip 0:47165eb4e54d 38 left.period(0.0066);
iLyngklip 0:47165eb4e54d 39 right.period(0.0066);
iLyngklip 0:47165eb4e54d 40 while(1){
iLyngklip 0:47165eb4e54d 41 myservo.position(1, 0);
iLyngklip 0:47165eb4e54d 42 wait_ms(delay);
iLyngklip 0:47165eb4e54d 43 G0 = sensor.distance(CM);
iLyngklip 0:47165eb4e54d 44 i = i + A;
iLyngklip 0:47165eb4e54d 45 myservo.position(1, 180);
iLyngklip 0:47165eb4e54d 46 G180 = sensor.distance(CM);
iLyngklip 0:47165eb4e54d 47 wait_ms(delay);
iLyngklip 0:47165eb4e54d 48 i = i + A;
iLyngklip 0:47165eb4e54d 49
iLyngklip 0:47165eb4e54d 50 pc.printf("G0: %ld G15: %ld G30: %ld G45: %ld G60: %ld G75: %ld G90: %ld G105: %ld G120: %ld G135: %ld G150: %ld G165: %ld G180: %ld \r\n", G0, G15, G30, G45, G60, G75, G90, G105, G120, G135, G150, G165, G180);
iLyngklip 0:47165eb4e54d 51
iLyngklip 0:47165eb4e54d 52
iLyngklip 0:47165eb4e54d 53 if(G0 > G180){
iLyngklip 0:47165eb4e54d 54 left.write(0.4);
iLyngklip 0:47165eb4e54d 55 right.write(0.2);
iLyngklip 0:47165eb4e54d 56 }
iLyngklip 0:47165eb4e54d 57 else if(G180 > G0){
iLyngklip 0:47165eb4e54d 58 left.write(0.2);
iLyngklip 0:47165eb4e54d 59 right.write(0.4);
iLyngklip 0:47165eb4e54d 60 }
iLyngklip 0:47165eb4e54d 61
iLyngklip 0:47165eb4e54d 62
iLyngklip 0:47165eb4e54d 63
iLyngklip 0:47165eb4e54d 64
iLyngklip 0:47165eb4e54d 65
iLyngklip 0:47165eb4e54d 66
iLyngklip 0:47165eb4e54d 67 if(i >= 180 || i+20 >= 180){
iLyngklip 0:47165eb4e54d 68 i=0;
iLyngklip 0:47165eb4e54d 69 }//if
iLyngklip 0:47165eb4e54d 70 } // while
iLyngklip 0:47165eb4e54d 71 } // main
iLyngklip 0:47165eb4e54d 72