Mathias Lyngklip / Mbed 2 deprecated HACKATHON2015-2

diller

Dependencies:   HCSR04 mbed tsi_sensor

Fork of frdm_tsi_slider by Mathias Riis

main.cpp@2:dfc1315b4b03, 2015-04-11 (annotated)

Committer:
iLyngklip
Date:
Sat Apr 11 10:41:09 2015 +0000
Revision:
2:dfc1315b4b03
Parent:
1:06bd2e196518 
diller;

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));
iLyngklip 2:dfc1315b4b03 61 */
iLyngklip 2:dfc1315b4b03 62 char diller = BT.getc();
iLyngklip 2:dfc1315b4b03 63 BT.putc(diller);
kasperhangard 0:5e7a5b5b6922 64 // Break out of loop if 'p'
iLyngklip 2:dfc1315b4b03 65 if(diller == 'p' && pilot == true){
iLyngklip 2:dfc1315b4b03 66 pilot = false;
kasperhangard 0:5e7a5b5b6922 67 }
kasperhangard 0:5e7a5b5b6922 68
kasperhangard 0:5e7a5b5b6922 69 //Drej til venstre for at rette op
kasperhangard 0:5e7a5b5b6922 70 if(dRIGHT > 35){
kasperhangard 0:5e7a5b5b6922 71 leftForward = 0.75;
kasperhangard 0:5e7a5b5b6922 72 leftBackward = 0.25;
kasperhangard 0:5e7a5b5b6922 73 rightForward = 0.25;
kasperhangard 0:5e7a5b5b6922 74 rightBackward = 0.75;
kasperhangard 0:5e7a5b5b6922 75 }
kasperhangard 0:5e7a5b5b6922 76
kasperhangard 0:5e7a5b5b6922 77 else if(dLEFT > 35){
kasperhangard 0:5e7a5b5b6922 78 leftForward = 0.25;
kasperhangard 0:5e7a5b5b6922 79 leftBackward = 0.75;
kasperhangard 0:5e7a5b5b6922 80 rightForward = 0.75;
kasperhangard 0:5e7a5b5b6922 81 rightBackward = 0.25;
kasperhangard 0:5e7a5b5b6922 82 }
kasperhangard 0:5e7a5b5b6922 83
kasperhangard 0:5e7a5b5b6922 84 else if(dRIGHT > 50){
kasperhangard 0:5e7a5b5b6922 85 leftForward = 0.1;
kasperhangard 0:5e7a5b5b6922 86 leftBackward = 0.9;
kasperhangard 0:5e7a5b5b6922 87 rightForward = 0.9;
kasperhangard 0:5e7a5b5b6922 88 rightBackward = 0.1;
kasperhangard 0:5e7a5b5b6922 89 }
kasperhangard 0:5e7a5b5b6922 90
kasperhangard 0:5e7a5b5b6922 91 else if(dLEFT > 50){
kasperhangard 0:5e7a5b5b6922 92 leftForward = 0.1;
kasperhangard 0:5e7a5b5b6922 93 leftBackward = 0.9;
kasperhangard 0:5e7a5b5b6922 94 rightForward = 0.9;
kasperhangard 0:5e7a5b5b6922 95 rightBackward = 0.1;
kasperhangard 0:5e7a5b5b6922 96 }else{
kasperhangard 0:5e7a5b5b6922 97 leftForward = 1;
kasperhangard 0:5e7a5b5b6922 98 leftBackward = 0;
kasperhangard 0:5e7a5b5b6922 99 rightForward = 1;
kasperhangard 0:5e7a5b5b6922 100 rightBackward = 0;
kasperhangard 0:5e7a5b5b6922 101 }
kasperhangard 0:5e7a5b5b6922 102 }
kasperhangard 0:5e7a5b5b6922 103
kasperhangard 0:5e7a5b5b6922 104 void motorControl(char input) {
kasperhangard 0:5e7a5b5b6922 105 switch (input) {
kasperhangard 0:5e7a5b5b6922 106 case 'w':
iLyngklip 2:dfc1315b4b03 107 rightSpeed = 0;
iLyngklip 2:dfc1315b4b03 108 leftSpeed = 0;
kasperhangard 0:5e7a5b5b6922 109 BT.printf("FORWARDS!");
kasperhangard 0:5e7a5b5b6922 110 if (frontSpeed < 0.99) {
kasperhangard 0:5e7a5b5b6922 111 frontSpeed = frontSpeed + 0.25f;
kasperhangard 0:5e7a5b5b6922 112 }
kasperhangard 0:5e7a5b5b6922 113 if (backSpeed > 0.1) {
kasperhangard 0:5e7a5b5b6922 114 backSpeed = backSpeed - 0.25f;
kasperhangard 0:5e7a5b5b6922 115 }
kasperhangard 0:5e7a5b5b6922 116 leftForward = frontSpeed;
kasperhangard 0:5e7a5b5b6922 117 leftBackward = backSpeed;
kasperhangard 0:5e7a5b5b6922 118 rightForward = frontSpeed;
kasperhangard 0:5e7a5b5b6922 119 rightBackward = backSpeed;
iLyngklip 2:dfc1315b4b03 120
kasperhangard 0:5e7a5b5b6922 121 break;
kasperhangard 0:5e7a5b5b6922 122
kasperhangard 0:5e7a5b5b6922 123 case 's':
iLyngklip 2:dfc1315b4b03 124 rightSpeed = 0;
iLyngklip 2:dfc1315b4b03 125 leftSpeed = 0;
kasperhangard 0:5e7a5b5b6922 126 if (frontSpeed > 0.1) {
kasperhangard 0:5e7a5b5b6922 127 frontSpeed = frontSpeed - 0.25f;
kasperhangard 0:5e7a5b5b6922 128 }
kasperhangard 0:5e7a5b5b6922 129 if (backSpeed < 0.99) {
kasperhangard 0:5e7a5b5b6922 130 backSpeed = backSpeed + 0.25f;
kasperhangard 0:5e7a5b5b6922 131 }
kasperhangard 0:5e7a5b5b6922 132 leftForward = frontSpeed;
kasperhangard 0:5e7a5b5b6922 133 leftBackward = backSpeed;
kasperhangard 0:5e7a5b5b6922 134 rightForward = frontSpeed;
kasperhangard 0:5e7a5b5b6922 135 rightBackward = backSpeed;
kasperhangard 0:5e7a5b5b6922 136 break;
kasperhangard 0:5e7a5b5b6922 137
kasperhangard 0:5e7a5b5b6922 138 case 'a':
kasperhangard 0:5e7a5b5b6922 139 if (leftSpeed > 0.1) {
Risifutti 1:06bd2e196518 140 leftSpeed = leftSpeed - 0.10f;
kasperhangard 0:5e7a5b5b6922 141 }
kasperhangard 0:5e7a5b5b6922 142 if (rightSpeed < 0.99) {
Risifutti 1:06bd2e196518 143 rightSpeed = rightSpeed + 0.10f;
kasperhangard 0:5e7a5b5b6922 144 }
kasperhangard 0:5e7a5b5b6922 145 leftForward = rightSpeed;
kasperhangard 0:5e7a5b5b6922 146 leftBackward = leftSpeed;
kasperhangard 0:5e7a5b5b6922 147 rightForward = leftSpeed;
kasperhangard 0:5e7a5b5b6922 148 rightBackward = rightSpeed;
kasperhangard 0:5e7a5b5b6922 149 break;
kasperhangard 0:5e7a5b5b6922 150
kasperhangard 0:5e7a5b5b6922 151 case 'd':
kasperhangard 0:5e7a5b5b6922 152 if (leftSpeed < 0.99) {
Risifutti 1:06bd2e196518 153 leftSpeed = leftSpeed + 0.10f;
kasperhangard 0:5e7a5b5b6922 154 }
kasperhangard 0:5e7a5b5b6922 155 if (rightSpeed > 0.1) {
Risifutti 1:06bd2e196518 156 rightSpeed = rightSpeed - 0.10f;
kasperhangard 0:5e7a5b5b6922 157 }
kasperhangard 0:5e7a5b5b6922 158 leftForward = rightSpeed;
kasperhangard 0:5e7a5b5b6922 159 leftBackward = leftSpeed;
kasperhangard 0:5e7a5b5b6922 160 rightForward = leftSpeed;
kasperhangard 0:5e7a5b5b6922 161 rightBackward = rightSpeed;
kasperhangard 0:5e7a5b5b6922 162 break;
kasperhangard 0:5e7a5b5b6922 163
kasperhangard 0:5e7a5b5b6922 164 case 'r':
kasperhangard 0:5e7a5b5b6922 165 // Go up
kasperhangard 0:5e7a5b5b6922 166 heightDown = 0;
kasperhangard 0:5e7a5b5b6922 167 heightUp.write(0.5);
kasperhangard 0:5e7a5b5b6922 168 wait(0.25);
kasperhangard 0:5e7a5b5b6922 169 heightUp = 0;
kasperhangard 0:5e7a5b5b6922 170 break;
kasperhangard 0:5e7a5b5b6922 171
kasperhangard 0:5e7a5b5b6922 172 case 'f':
kasperhangard 0:5e7a5b5b6922 173 // Down
kasperhangard 0:5e7a5b5b6922 174 heightUp = 0;
kasperhangard 0:5e7a5b5b6922 175 heightDown.write(0.5);
kasperhangard 0:5e7a5b5b6922 176 wait(0.25);
kasperhangard 0:5e7a5b5b6922 177 heightDown = 0;
kasperhangard 0:5e7a5b5b6922 178 break;
kasperhangard 0:5e7a5b5b6922 179
kasperhangard 0:5e7a5b5b6922 180 case 'g':
kasperhangard 0:5e7a5b5b6922 181 // Close the grab
kasperhangard 0:5e7a5b5b6922 182 grabOut = 0;
kasperhangard 0:5e7a5b5b6922 183 grabIn.write(0.5);
kasperhangard 0:5e7a5b5b6922 184 wait(0.3);
kasperhangard 0:5e7a5b5b6922 185 grabIn = 0;
kasperhangard 0:5e7a5b5b6922 186 break;
kasperhangard 0:5e7a5b5b6922 187 case 'h':
kasperhangard 0:5e7a5b5b6922 188 // Open the grab
kasperhangard 0:5e7a5b5b6922 189 grabIn = 0;
kasperhangard 0:5e7a5b5b6922 190 grabOut.write(0.5);
kasperhangard 0:5e7a5b5b6922 191 wait(0.3);
kasperhangard 0:5e7a5b5b6922 192 grabOut = 0;
kasperhangard 0:5e7a5b5b6922 193 break;
iLyngklip 2:dfc1315b4b03 194
iLyngklip 2:dfc1315b4b03 195
kasperhangard 0:5e7a5b5b6922 196 case 'x':
kasperhangard 0:5e7a5b5b6922 197 frontSpeed = 0;
kasperhangard 0:5e7a5b5b6922 198 backSpeed = 0;
kasperhangard 0:5e7a5b5b6922 199 rightSpeed = 0;
kasperhangard 0:5e7a5b5b6922 200 leftSpeed = 0;
kasperhangard 0:5e7a5b5b6922 201 leftForward = 0;
kasperhangard 0:5e7a5b5b6922 202 rightForward = 0;
kasperhangard 0:5e7a5b5b6922 203 leftBackward = 0;
kasperhangard 0:5e7a5b5b6922 204 rightBackward = 0;
kasperhangard 0:5e7a5b5b6922 205 grabIn = 0;
kasperhangard 0:5e7a5b5b6922 206 grabOut = 0;
kasperhangard 0:5e7a5b5b6922 207 heightUp = 0;
kasperhangard 0:5e7a5b5b6922 208 heightDown = 0;
kasperhangard 0:5e7a5b5b6922 209 break;
iLyngklip 2:dfc1315b4b03 210
kasperhangard 0:5e7a5b5b6922 211 case 'p':
kasperhangard 0:5e7a5b5b6922 212 if(pilot == false){
kasperhangard 0:5e7a5b5b6922 213 pilot = true;
kasperhangard 0:5e7a5b5b6922 214 while(1){
iLyngklip 2:dfc1315b4b03 215 selfDrive();
kasperhangard 0:5e7a5b5b6922 216 }
kasperhangard 0:5e7a5b5b6922 217 }
kasperhangard 0:5e7a5b5b6922 218 break;
iLyngklip 2:dfc1315b4b03 219
kasperhangard 0:5e7a5b5b6922 220 }
kasperhangard 0:5e7a5b5b6922 221 }
kasperhangard 0:5e7a5b5b6922 222 int main(void) {
kasperhangard 0:5e7a5b5b6922 223 LED = 0.1;
kasperhangard 0:5e7a5b5b6922 224 leftForward = 0;
kasperhangard 0:5e7a5b5b6922 225 rightForward = 0;
kasperhangard 0:5e7a5b5b6922 226 leftBackward = 0;
kasperhangard 0:5e7a5b5b6922 227 rightBackward = 0;
kasperhangard 0:5e7a5b5b6922 228 while (1) {
kasperhangard 0:5e7a5b5b6922 229
kasperhangard 0:5e7a5b5b6922 230 command = BT.getc();
kasperhangard 0:5e7a5b5b6922 231 BT.putc(command);
iLyngklip 2:dfc1315b4b03 232 motorControl(command);
kasperhangard 0:5e7a5b5b6922 233 }
kasperhangard 0:5e7a5b5b6922 234 }