Mathias Lyngklip / Mbed 2 deprecated HACKATHON2015-2

diller

Dependencies:   HCSR04 mbed tsi_sensor

Fork of frdm_tsi_slider by Mathias Riis

main.cpp@0:5e7a5b5b6922, 2015-04-11 (annotated)

Committer:
kasperhangard
Date:
Sat Apr 11 08:57:27 2015 +0000
Revision:
0:5e7a5b5b6922
Child:
1:06bd2e196518
Final

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kasperhangard 0:5e7a5b5b6922 1 #include "mbed.h"
kasperhangard 0:5e7a5b5b6922 2 #include "tsi_sensor.h"
kasperhangard 0:5e7a5b5b6922 3 #include <string>
kasperhangard 0:5e7a5b5b6922 4 #include "HCSR04.h"
kasperhangard 0:5e7a5b5b6922 5
kasperhangard 0:5e7a5b5b6922 6 /* This defines will be replaced by PinNames soon
kasperhangard 0:5e7a5b5b6922 7 #if defined (TARGET_KL25Z) || defined (TARGET_KL46Z)
kasperhangard 0:5e7a5b5b6922 8 #define ELEC0 9
kasperhangard 0:5e7a5b5b6922 9 #define ELEC1 10
kasperhangard 0:5e7a5b5b6922 10 #elif defined (TARGET_KL05Z)
kasperhangard 0:5e7a5b5b6922 11 #define ELEC0 9
kasperhangard 0:5e7a5b5b6922 12 #define ELEC1 8
kasperhangard 0:5e7a5b5b6922 13 #else
kasperhangard 0:5e7a5b5b6922 14 #error TARGET NOT DEFINED
kasperhangard 0:5e7a5b5b6922 15 #endif*/
kasperhangard 0:5e7a5b5b6922 16
kasperhangard 0:5e7a5b5b6922 17 PwmOut LED(LED1);
kasperhangard 0:5e7a5b5b6922 18 PwmOut leftForward(PTD5);
kasperhangard 0:5e7a5b5b6922 19 PwmOut leftBackward(PTA13);
kasperhangard 0:5e7a5b5b6922 20 PwmOut rightForward(PTA5);
kasperhangard 0:5e7a5b5b6922 21 PwmOut rightBackward(PTA4);
kasperhangard 0:5e7a5b5b6922 22
kasperhangard 0:5e7a5b5b6922 23 PwmOut grabIn(PTE30);
kasperhangard 0:5e7a5b5b6922 24 PwmOut grabOut(PTC1);
kasperhangard 0:5e7a5b5b6922 25 PwmOut heightUp(PTD4);
kasperhangard 0:5e7a5b5b6922 26 PwmOut heightDown(PTA12);
kasperhangard 0:5e7a5b5b6922 27
kasperhangard 0:5e7a5b5b6922 28 // Defines the sensors (Left and Right)
kasperhangard 0:5e7a5b5b6922 29 HCSR04 sensorLEFT(PTC12, PTC13);
kasperhangard 0:5e7a5b5b6922 30 HCSR04 sensorRIGHT(PTC16, PTC17);
kasperhangard 0:5e7a5b5b6922 31
kasperhangard 0:5e7a5b5b6922 32 Serial pc(USBTX, USBRX);
kasperhangard 0:5e7a5b5b6922 33 Serial BT(PTC4, PTC3);
kasperhangard 0:5e7a5b5b6922 34 char command = 0;
kasperhangard 0:5e7a5b5b6922 35 char store;
kasperhangard 0:5e7a5b5b6922 36 float frontSpeed = 0;
kasperhangard 0:5e7a5b5b6922 37 float backSpeed = 0;
kasperhangard 0:5e7a5b5b6922 38 float rightSpeed = 0;
kasperhangard 0:5e7a5b5b6922 39 float leftSpeed = 0;
kasperhangard 0:5e7a5b5b6922 40 bool pilot = false;
kasperhangard 0:5e7a5b5b6922 41
kasperhangard 0:5e7a5b5b6922 42
kasperhangard 0:5e7a5b5b6922 43 int distLeft(int dLEFT){
kasperhangard 0:5e7a5b5b6922 44 return dLEFT;
kasperhangard 0:5e7a5b5b6922 45 }
kasperhangard 0:5e7a5b5b6922 46
kasperhangard 0:5e7a5b5b6922 47 // Get the right distance variable
kasperhangard 0:5e7a5b5b6922 48 int distRight(int dRIGHT){
kasperhangard 0:5e7a5b5b6922 49 return dRIGHT;
kasperhangard 0:5e7a5b5b6922 50 }
kasperhangard 0:5e7a5b5b6922 51
kasperhangard 0:5e7a5b5b6922 52 //SELVKØRENDE I MØRKET!
kasperhangard 0:5e7a5b5b6922 53 void selfDrive(){
kasperhangard 0:5e7a5b5b6922 54
kasperhangard 0:5e7a5b5b6922 55 int dLEFT = sensorLEFT.distance(CM);
kasperhangard 0:5e7a5b5b6922 56 int dRIGHT = sensorRIGHT.distance(CM);
kasperhangard 0:5e7a5b5b6922 57 /*
kasperhangard 0:5e7a5b5b6922 58 // Writes the left and right distance variable
kasperhangard 0:5e7a5b5b6922 59 pc.printf("SENSOR Left: %d \n\r\v",distLeft(dLEFT));
kasperhangard 0:5e7a5b5b6922 60 pc.printf("SENSOR Right: %d \n\r\v",distRight(dRIGHT));
kasperhangard 0:5e7a5b5b6922 61 */
kasperhangard 0:5e7a5b5b6922 62 // Break out of loop if 'p'
kasperhangard 0:5e7a5b5b6922 63 if(BT.getc() == 'p' && pilot == true){
kasperhangard 0:5e7a5b5b6922 64 pilot == false;
kasperhangard 0:5e7a5b5b6922 65 }
kasperhangard 0:5e7a5b5b6922 66
kasperhangard 0:5e7a5b5b6922 67 //Drej til venstre for at rette op
kasperhangard 0:5e7a5b5b6922 68 if(dRIGHT > 35){
kasperhangard 0:5e7a5b5b6922 69 leftForward = 0.75;
kasperhangard 0:5e7a5b5b6922 70 leftBackward = 0.25;
kasperhangard 0:5e7a5b5b6922 71 rightForward = 0.25;
kasperhangard 0:5e7a5b5b6922 72 rightBackward = 0.75;
kasperhangard 0:5e7a5b5b6922 73 }
kasperhangard 0:5e7a5b5b6922 74
kasperhangard 0:5e7a5b5b6922 75 else if(dLEFT > 35){
kasperhangard 0:5e7a5b5b6922 76 leftForward = 0.25;
kasperhangard 0:5e7a5b5b6922 77 leftBackward = 0.75;
kasperhangard 0:5e7a5b5b6922 78 rightForward = 0.75;
kasperhangard 0:5e7a5b5b6922 79 rightBackward = 0.25;
kasperhangard 0:5e7a5b5b6922 80 }
kasperhangard 0:5e7a5b5b6922 81
kasperhangard 0:5e7a5b5b6922 82 else if(dRIGHT > 50){
kasperhangard 0:5e7a5b5b6922 83 leftForward = 0.1;
kasperhangard 0:5e7a5b5b6922 84 leftBackward = 0.9;
kasperhangard 0:5e7a5b5b6922 85 rightForward = 0.9;
kasperhangard 0:5e7a5b5b6922 86 rightBackward = 0.1;
kasperhangard 0:5e7a5b5b6922 87 }
kasperhangard 0:5e7a5b5b6922 88
kasperhangard 0:5e7a5b5b6922 89 else if(dLEFT > 50){
kasperhangard 0:5e7a5b5b6922 90 leftForward = 0.1;
kasperhangard 0:5e7a5b5b6922 91 leftBackward = 0.9;
kasperhangard 0:5e7a5b5b6922 92 rightForward = 0.9;
kasperhangard 0:5e7a5b5b6922 93 rightBackward = 0.1;
kasperhangard 0:5e7a5b5b6922 94 }else{
kasperhangard 0:5e7a5b5b6922 95 leftForward = 1;
kasperhangard 0:5e7a5b5b6922 96 leftBackward = 0;
kasperhangard 0:5e7a5b5b6922 97 rightForward = 1;
kasperhangard 0:5e7a5b5b6922 98 rightBackward = 0;
kasperhangard 0:5e7a5b5b6922 99 }
kasperhangard 0:5e7a5b5b6922 100 }
kasperhangard 0:5e7a5b5b6922 101
kasperhangard 0:5e7a5b5b6922 102 void motorControl(char input) {
kasperhangard 0:5e7a5b5b6922 103 switch (input) {
kasperhangard 0:5e7a5b5b6922 104 case 'w':
kasperhangard 0:5e7a5b5b6922 105 BT.printf("FORWARDS!");
kasperhangard 0:5e7a5b5b6922 106 if (frontSpeed < 0.99) {
kasperhangard 0:5e7a5b5b6922 107 frontSpeed = frontSpeed + 0.25f;
kasperhangard 0:5e7a5b5b6922 108 }
kasperhangard 0:5e7a5b5b6922 109 if (backSpeed > 0.1) {
kasperhangard 0:5e7a5b5b6922 110 backSpeed = backSpeed - 0.25f;
kasperhangard 0:5e7a5b5b6922 111 }
kasperhangard 0:5e7a5b5b6922 112 leftForward = frontSpeed;
kasperhangard 0:5e7a5b5b6922 113 leftBackward = backSpeed;
kasperhangard 0:5e7a5b5b6922 114 rightForward = frontSpeed;
kasperhangard 0:5e7a5b5b6922 115 rightBackward = backSpeed;
kasperhangard 0:5e7a5b5b6922 116 break;
kasperhangard 0:5e7a5b5b6922 117
kasperhangard 0:5e7a5b5b6922 118 case 's':
kasperhangard 0:5e7a5b5b6922 119 if (frontSpeed > 0.1) {
kasperhangard 0:5e7a5b5b6922 120 frontSpeed = frontSpeed - 0.25f;
kasperhangard 0:5e7a5b5b6922 121 }
kasperhangard 0:5e7a5b5b6922 122 if (backSpeed < 0.99) {
kasperhangard 0:5e7a5b5b6922 123 backSpeed = backSpeed + 0.25f;
kasperhangard 0:5e7a5b5b6922 124 }
kasperhangard 0:5e7a5b5b6922 125 leftForward = frontSpeed;
kasperhangard 0:5e7a5b5b6922 126 leftBackward = backSpeed;
kasperhangard 0:5e7a5b5b6922 127 rightForward = frontSpeed;
kasperhangard 0:5e7a5b5b6922 128 rightBackward = backSpeed;
kasperhangard 0:5e7a5b5b6922 129 break;
kasperhangard 0:5e7a5b5b6922 130
kasperhangard 0:5e7a5b5b6922 131 case 'a':
kasperhangard 0:5e7a5b5b6922 132 if (leftSpeed > 0.1) {
kasperhangard 0:5e7a5b5b6922 133 leftSpeed = leftSpeed - 0.25f;
kasperhangard 0:5e7a5b5b6922 134 }
kasperhangard 0:5e7a5b5b6922 135 if (rightSpeed < 0.99) {
kasperhangard 0:5e7a5b5b6922 136 rightSpeed = rightSpeed + 0.25f;
kasperhangard 0:5e7a5b5b6922 137 }
kasperhangard 0:5e7a5b5b6922 138 leftForward = rightSpeed;
kasperhangard 0:5e7a5b5b6922 139 leftBackward = leftSpeed;
kasperhangard 0:5e7a5b5b6922 140 rightForward = leftSpeed;
kasperhangard 0:5e7a5b5b6922 141 rightBackward = rightSpeed;
kasperhangard 0:5e7a5b5b6922 142 break;
kasperhangard 0:5e7a5b5b6922 143
kasperhangard 0:5e7a5b5b6922 144 case 'd':
kasperhangard 0:5e7a5b5b6922 145 if (leftSpeed < 0.99) {
kasperhangard 0:5e7a5b5b6922 146 leftSpeed = leftSpeed + 0.25f;
kasperhangard 0:5e7a5b5b6922 147 }
kasperhangard 0:5e7a5b5b6922 148 if (rightSpeed > 0.1) {
kasperhangard 0:5e7a5b5b6922 149 rightSpeed = rightSpeed - 0.25f;
kasperhangard 0:5e7a5b5b6922 150 }
kasperhangard 0:5e7a5b5b6922 151 leftForward = rightSpeed;
kasperhangard 0:5e7a5b5b6922 152 leftBackward = leftSpeed;
kasperhangard 0:5e7a5b5b6922 153 rightForward = leftSpeed;
kasperhangard 0:5e7a5b5b6922 154 rightBackward = rightSpeed;
kasperhangard 0:5e7a5b5b6922 155 break;
kasperhangard 0:5e7a5b5b6922 156
kasperhangard 0:5e7a5b5b6922 157 case 'r':
kasperhangard 0:5e7a5b5b6922 158 // Go up
kasperhangard 0:5e7a5b5b6922 159 heightDown = 0;
kasperhangard 0:5e7a5b5b6922 160 heightUp.write(0.5);
kasperhangard 0:5e7a5b5b6922 161 wait(0.25);
kasperhangard 0:5e7a5b5b6922 162 heightUp = 0;
kasperhangard 0:5e7a5b5b6922 163 break;
kasperhangard 0:5e7a5b5b6922 164
kasperhangard 0:5e7a5b5b6922 165 case 'f':
kasperhangard 0:5e7a5b5b6922 166 // Down
kasperhangard 0:5e7a5b5b6922 167 heightUp = 0;
kasperhangard 0:5e7a5b5b6922 168 heightDown.write(0.5);
kasperhangard 0:5e7a5b5b6922 169 wait(0.25);
kasperhangard 0:5e7a5b5b6922 170 heightDown = 0;
kasperhangard 0:5e7a5b5b6922 171 break;
kasperhangard 0:5e7a5b5b6922 172
kasperhangard 0:5e7a5b5b6922 173 case 'g':
kasperhangard 0:5e7a5b5b6922 174 // Close the grab
kasperhangard 0:5e7a5b5b6922 175 grabOut = 0;
kasperhangard 0:5e7a5b5b6922 176 grabIn.write(0.5);
kasperhangard 0:5e7a5b5b6922 177 wait(0.3);
kasperhangard 0:5e7a5b5b6922 178 grabIn = 0;
kasperhangard 0:5e7a5b5b6922 179 break;
kasperhangard 0:5e7a5b5b6922 180 case 'h':
kasperhangard 0:5e7a5b5b6922 181 // Open the grab
kasperhangard 0:5e7a5b5b6922 182 grabIn = 0;
kasperhangard 0:5e7a5b5b6922 183 grabOut.write(0.5);
kasperhangard 0:5e7a5b5b6922 184 wait(0.3);
kasperhangard 0:5e7a5b5b6922 185 grabOut = 0;
kasperhangard 0:5e7a5b5b6922 186 break;
kasperhangard 0:5e7a5b5b6922 187 case 'x':
kasperhangard 0:5e7a5b5b6922 188 frontSpeed = 0;
kasperhangard 0:5e7a5b5b6922 189 backSpeed = 0;
kasperhangard 0:5e7a5b5b6922 190 rightSpeed = 0;
kasperhangard 0:5e7a5b5b6922 191 leftSpeed = 0;
kasperhangard 0:5e7a5b5b6922 192 leftForward = 0;
kasperhangard 0:5e7a5b5b6922 193 rightForward = 0;
kasperhangard 0:5e7a5b5b6922 194 leftBackward = 0;
kasperhangard 0:5e7a5b5b6922 195 rightBackward = 0;
kasperhangard 0:5e7a5b5b6922 196 grabIn = 0;
kasperhangard 0:5e7a5b5b6922 197 grabOut = 0;
kasperhangard 0:5e7a5b5b6922 198 heightUp = 0;
kasperhangard 0:5e7a5b5b6922 199 heightDown = 0;
kasperhangard 0:5e7a5b5b6922 200 break;
kasperhangard 0:5e7a5b5b6922 201 case 'p':
kasperhangard 0:5e7a5b5b6922 202 if(pilot == false){
kasperhangard 0:5e7a5b5b6922 203 pilot = true;
kasperhangard 0:5e7a5b5b6922 204 while(1){
kasperhangard 0:5e7a5b5b6922 205 selfDrive();
kasperhangard 0:5e7a5b5b6922 206 }
kasperhangard 0:5e7a5b5b6922 207 }
kasperhangard 0:5e7a5b5b6922 208 break;
kasperhangard 0:5e7a5b5b6922 209 }
kasperhangard 0:5e7a5b5b6922 210 }
kasperhangard 0:5e7a5b5b6922 211 int main(void) {
kasperhangard 0:5e7a5b5b6922 212 LED = 0.1;
kasperhangard 0:5e7a5b5b6922 213 leftForward = 0;
kasperhangard 0:5e7a5b5b6922 214 rightForward = 0;
kasperhangard 0:5e7a5b5b6922 215 leftBackward = 0;
kasperhangard 0:5e7a5b5b6922 216 rightBackward = 0;
kasperhangard 0:5e7a5b5b6922 217 while (1) {
kasperhangard 0:5e7a5b5b6922 218
kasperhangard 0:5e7a5b5b6922 219 command = BT.getc();
kasperhangard 0:5e7a5b5b6922 220 BT.putc(command);
kasperhangard 0:5e7a5b5b6922 221 motorControl(command);
kasperhangard 0:5e7a5b5b6922 222 }
kasperhangard 0:5e7a5b5b6922 223 }