Mathias Lyngklip / Mbed 2 deprecated RoboticHackathon2

hej

Dependencies:   dillerdasker mbed Rfid

Fork of RoboticHackathon by Mathias Lyngklip

main.cpp@2:b996c95912b5, 2014-04-07 (annotated)

Committer:
iLyngklip
Date:
Mon Apr 07 06:24:19 2014 +0000
Revision:
2:b996c95912b5
Parent:
1:e854d5c12659 
Final version 2. drive

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 "PwmOut.h"
iLyngklip 1:e854d5c12659 4 #include "hack_motor.h"
iLyngklip 0:47165eb4e54d 5
iLyngklip 0:47165eb4e54d 6
iLyngklip 0:47165eb4e54d 7 Serial pc(USBTX, USBRX);
iLyngklip 1:e854d5c12659 8 HCSR04 sensor(PTA13, PTD5, PTD0, PTD2);
iLyngklip 1:e854d5c12659 9
iLyngklip 1:e854d5c12659 10 Wheel robot;
iLyngklip 2:b996c95912b5 11 int control = 0;
iLyngklip 2:b996c95912b5 12 int L1 = 0;
iLyngklip 2:b996c95912b5 13 int L2 = 0;
iLyngklip 0:47165eb4e54d 14 int main(){
iLyngklip 1:e854d5c12659 15
iLyngklip 1:e854d5c12659 16
iLyngklip 0:47165eb4e54d 17 pc.baud(9600);
iLyngklip 1:e854d5c12659 18
iLyngklip 2:b996c95912b5 19 while(control == 0){
iLyngklip 1:e854d5c12659 20 robot.init();
iLyngklip 1:e854d5c12659 21 char c;
iLyngklip 1:e854d5c12659 22 pc.printf(" speed 1, 2 & 3, frem w, bagud s, venstre a & hoejre d");
iLyngklip 1:e854d5c12659 23
iLyngklip 1:e854d5c12659 24 {
iLyngklip 2:b996c95912b5 25 c = pc.getc();
iLyngklip 1:e854d5c12659 26 switch (c)
iLyngklip 1:e854d5c12659 27 {
iLyngklip 1:e854d5c12659 28 case 0x31:
iLyngklip 1:e854d5c12659 29 robot.speed = 1.0;
iLyngklip 1:e854d5c12659 30 pc.printf("1");
iLyngklip 1:e854d5c12659 31 break;
iLyngklip 1:e854d5c12659 32
iLyngklip 1:e854d5c12659 33 case 0x32:
iLyngklip 1:e854d5c12659 34 robot.speed = 0.5;
iLyngklip 1:e854d5c12659 35 pc.printf("2");
iLyngklip 1:e854d5c12659 36 break;
iLyngklip 1:e854d5c12659 37
iLyngklip 1:e854d5c12659 38 case 0x33:
iLyngklip 1:e854d5c12659 39 robot.speed = 0.25;
iLyngklip 1:e854d5c12659 40 pc.printf("3");
iLyngklip 1:e854d5c12659 41 break;
iLyngklip 1:e854d5c12659 42
iLyngklip 2:b996c95912b5 43 case 's':
iLyngklip 1:e854d5c12659 44 robot.FW();
iLyngklip 2:b996c95912b5 45 pc.printf("s");
iLyngklip 1:e854d5c12659 46 break;
iLyngklip 1:e854d5c12659 47
iLyngklip 2:b996c95912b5 48 case 'w':
iLyngklip 1:e854d5c12659 49 robot.BW();
iLyngklip 2:b996c95912b5 50 pc.printf("w");
iLyngklip 1:e854d5c12659 51 break;
iLyngklip 1:e854d5c12659 52
iLyngklip 1:e854d5c12659 53 case 0x61:
iLyngklip 1:e854d5c12659 54 robot.left();
iLyngklip 1:e854d5c12659 55 pc.printf("a");
iLyngklip 2:b996c95912b5 56 wait(0.1);
iLyngklip 2:b996c95912b5 57 robot.stop();
iLyngklip 2:b996c95912b5 58
iLyngklip 1:e854d5c12659 59 break;
iLyngklip 1:e854d5c12659 60
iLyngklip 1:e854d5c12659 61 case 0x64:
iLyngklip 1:e854d5c12659 62 robot.right();
iLyngklip 2:b996c95912b5 63 pc.printf("d");
iLyngklip 2:b996c95912b5 64 wait(0.1);
iLyngklip 2:b996c95912b5 65 robot.stop();
iLyngklip 1:e854d5c12659 66 break;
iLyngklip 1:e854d5c12659 67
iLyngklip 1:e854d5c12659 68 case 'n':
iLyngklip 1:e854d5c12659 69 robot.open();
iLyngklip 1:e854d5c12659 70 pc.printf("n");
iLyngklip 1:e854d5c12659 71 break;
iLyngklip 1:e854d5c12659 72
iLyngklip 1:e854d5c12659 73 case 'm':
iLyngklip 1:e854d5c12659 74 robot.close();
iLyngklip 1:e854d5c12659 75 pc.printf("m");
iLyngklip 1:e854d5c12659 76 break;
iLyngklip 1:e854d5c12659 77
iLyngklip 1:e854d5c12659 78 case 'h':
iLyngklip 1:e854d5c12659 79 robot.hejs();
iLyngklip 1:e854d5c12659 80 pc.printf("h");
iLyngklip 1:e854d5c12659 81 break;
iLyngklip 1:e854d5c12659 82
iLyngklip 1:e854d5c12659 83 case 'b':
iLyngklip 1:e854d5c12659 84 robot.saenk();
iLyngklip 1:e854d5c12659 85 pc.printf("b");
iLyngklip 1:e854d5c12659 86 break;
iLyngklip 2:b996c95912b5 87
iLyngklip 2:b996c95912b5 88 case 'p':
iLyngklip 2:b996c95912b5 89 control = 1;
iLyngklip 2:b996c95912b5 90 break;
iLyngklip 2:b996c95912b5 91
iLyngklip 1:e854d5c12659 92
iLyngklip 1:e854d5c12659 93 default :
iLyngklip 1:e854d5c12659 94 robot.stop();
iLyngklip 1:e854d5c12659 95 break;
iLyngklip 1:e854d5c12659 96 }
iLyngklip 1:e854d5c12659 97 }
iLyngklip 2:b996c95912b5 98 }
iLyngklip 2:b996c95912b5 99 {
iLyngklip 2:b996c95912b5 100
iLyngklip 2:b996c95912b5 101
iLyngklip 2:b996c95912b5 102
iLyngklip 2:b996c95912b5 103
iLyngklip 2:b996c95912b5 104
iLyngklip 0:47165eb4e54d 105
iLyngklip 0:47165eb4e54d 106 } // while
iLyngklip 2:b996c95912b5 107
iLyngklip 2:b996c95912b5 108 while(control == 1){
iLyngklip 2:b996c95912b5 109
iLyngklip 2:b996c95912b5 110 L1 = sensor.Distance(CM); // Højre
iLyngklip 2:b996c95912b5 111 L2 = sensor.distance(CM); // Venstre
iLyngklip 2:b996c95912b5 112
iLyngklip 2:b996c95912b5 113 pc.printf("L1: %ld L2: %ld \r\n", L1, L2);
iLyngklip 2:b996c95912b5 114
iLyngklip 2:b996c95912b5 115
iLyngklip 2:b996c95912b5 116 if(L1 >= L2 && L1 > 50){
iLyngklip 2:b996c95912b5 117 robot.venSelv3(); // Højre skarpt
iLyngklip 2:b996c95912b5 118 }
iLyngklip 2:b996c95912b5 119
iLyngklip 2:b996c95912b5 120 if(L2 >= L1 && L2 > 50){
iLyngklip 2:b996c95912b5 121 robot.venSelv4(); // Venstre skarpt
iLyngklip 2:b996c95912b5 122 }
iLyngklip 2:b996c95912b5 123
iLyngklip 2:b996c95912b5 124 if(L1 >= L2){
iLyngklip 2:b996c95912b5 125 robot.venSelv1(); // Højre
iLyngklip 2:b996c95912b5 126 }
iLyngklip 2:b996c95912b5 127
iLyngklip 2:b996c95912b5 128
iLyngklip 2:b996c95912b5 129
iLyngklip 2:b996c95912b5 130 if(L2 > L1){
iLyngklip 2:b996c95912b5 131 robot.venSelv2(); // Venstre
iLyngklip 2:b996c95912b5 132 }
iLyngklip 2:b996c95912b5 133
iLyngklip 2:b996c95912b5 134 if(L1 > 65){
iLyngklip 2:b996c95912b5 135 control = 0;
iLyngklip 2:b996c95912b5 136 }
iLyngklip 2:b996c95912b5 137 }
iLyngklip 2:b996c95912b5 138
iLyngklip 2:b996c95912b5 139
iLyngklip 2:b996c95912b5 140
iLyngklip 2:b996c95912b5 141
iLyngklip 2:b996c95912b5 142
iLyngklip 0:47165eb4e54d 143 } // main
iLyngklip 0:47165eb4e54d 144