Mathias Lyngklip / Mbed 2 deprecated HACKATHON2015-2

diller

Dependencies:   HCSR04 mbed tsi_sensor

Fork of frdm_tsi_slider by Mathias Riis

main.cpp@1:06bd2e196518, 2015-04-11 (annotated)

Committer:
Risifutti
Date:
Sat Apr 11 09:51:14 2015 +0000
Revision:
1:06bd2e196518
Parent:
0:5e7a5b5b6922
Child:
2:dfc1315b4b03
NO1

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':
Risifutti 1:06bd2e196518 105 sideSpeed = 0;
kasperhangard 0:5e7a5b5b6922 106 BT.printf("FORWARDS!");
kasperhangard 0:5e7a5b5b6922 107 if (frontSpeed < 0.99) {
kasperhangard 0:5e7a5b5b6922 108 frontSpeed = frontSpeed + 0.25f;
kasperhangard 0:5e7a5b5b6922 109 }
kasperhangard 0:5e7a5b5b6922 110 if (backSpeed > 0.1) {
kasperhangard 0:5e7a5b5b6922 111 backSpeed = backSpeed - 0.25f;
kasperhangard 0:5e7a5b5b6922 112 }
kasperhangard 0:5e7a5b5b6922 113 leftForward = frontSpeed;
kasperhangard 0:5e7a5b5b6922 114 leftBackward = backSpeed;
kasperhangard 0:5e7a5b5b6922 115 rightForward = frontSpeed;
kasperhangard 0:5e7a5b5b6922 116 rightBackward = backSpeed;
kasperhangard 0:5e7a5b5b6922 117 break;
kasperhangard 0:5e7a5b5b6922 118
kasperhangard 0:5e7a5b5b6922 119 case 's':
Risifutti 1:06bd2e196518 120 sideSpeed = 0;
kasperhangard 0:5e7a5b5b6922 121 if (frontSpeed > 0.1) {
kasperhangard 0:5e7a5b5b6922 122 frontSpeed = frontSpeed - 0.25f;
kasperhangard 0:5e7a5b5b6922 123 }
kasperhangard 0:5e7a5b5b6922 124 if (backSpeed < 0.99) {
kasperhangard 0:5e7a5b5b6922 125 backSpeed = backSpeed + 0.25f;
kasperhangard 0:5e7a5b5b6922 126 }
kasperhangard 0:5e7a5b5b6922 127 leftForward = frontSpeed;
kasperhangard 0:5e7a5b5b6922 128 leftBackward = backSpeed;
kasperhangard 0:5e7a5b5b6922 129 rightForward = frontSpeed;
kasperhangard 0:5e7a5b5b6922 130 rightBackward = backSpeed;
kasperhangard 0:5e7a5b5b6922 131 break;
kasperhangard 0:5e7a5b5b6922 132
kasperhangard 0:5e7a5b5b6922 133 case 'a':
kasperhangard 0:5e7a5b5b6922 134 if (leftSpeed > 0.1) {
Risifutti 1:06bd2e196518 135 leftSpeed = leftSpeed - 0.10f;
kasperhangard 0:5e7a5b5b6922 136 }
kasperhangard 0:5e7a5b5b6922 137 if (rightSpeed < 0.99) {
Risifutti 1:06bd2e196518 138 rightSpeed = rightSpeed + 0.10f;
kasperhangard 0:5e7a5b5b6922 139 }
kasperhangard 0:5e7a5b5b6922 140 leftForward = rightSpeed;
kasperhangard 0:5e7a5b5b6922 141 leftBackward = leftSpeed;
kasperhangard 0:5e7a5b5b6922 142 rightForward = leftSpeed;
kasperhangard 0:5e7a5b5b6922 143 rightBackward = rightSpeed;
kasperhangard 0:5e7a5b5b6922 144 break;
kasperhangard 0:5e7a5b5b6922 145
kasperhangard 0:5e7a5b5b6922 146 case 'd':
kasperhangard 0:5e7a5b5b6922 147 if (leftSpeed < 0.99) {
Risifutti 1:06bd2e196518 148 leftSpeed = leftSpeed + 0.10f;
kasperhangard 0:5e7a5b5b6922 149 }
kasperhangard 0:5e7a5b5b6922 150 if (rightSpeed > 0.1) {
Risifutti 1:06bd2e196518 151 rightSpeed = rightSpeed - 0.10f;
kasperhangard 0:5e7a5b5b6922 152 }
kasperhangard 0:5e7a5b5b6922 153 leftForward = rightSpeed;
kasperhangard 0:5e7a5b5b6922 154 leftBackward = leftSpeed;
kasperhangard 0:5e7a5b5b6922 155 rightForward = leftSpeed;
kasperhangard 0:5e7a5b5b6922 156 rightBackward = rightSpeed;
kasperhangard 0:5e7a5b5b6922 157 break;
kasperhangard 0:5e7a5b5b6922 158
kasperhangard 0:5e7a5b5b6922 159 case 'r':
kasperhangard 0:5e7a5b5b6922 160 // Go up
kasperhangard 0:5e7a5b5b6922 161 heightDown = 0;
kasperhangard 0:5e7a5b5b6922 162 heightUp.write(0.5);
kasperhangard 0:5e7a5b5b6922 163 wait(0.25);
kasperhangard 0:5e7a5b5b6922 164 heightUp = 0;
kasperhangard 0:5e7a5b5b6922 165 break;
kasperhangard 0:5e7a5b5b6922 166
kasperhangard 0:5e7a5b5b6922 167 case 'f':
kasperhangard 0:5e7a5b5b6922 168 // Down
kasperhangard 0:5e7a5b5b6922 169 heightUp = 0;
kasperhangard 0:5e7a5b5b6922 170 heightDown.write(0.5);
kasperhangard 0:5e7a5b5b6922 171 wait(0.25);
kasperhangard 0:5e7a5b5b6922 172 heightDown = 0;
kasperhangard 0:5e7a5b5b6922 173 break;
kasperhangard 0:5e7a5b5b6922 174
kasperhangard 0:5e7a5b5b6922 175 case 'g':
kasperhangard 0:5e7a5b5b6922 176 // Close the grab
kasperhangard 0:5e7a5b5b6922 177 grabOut = 0;
kasperhangard 0:5e7a5b5b6922 178 grabIn.write(0.5);
kasperhangard 0:5e7a5b5b6922 179 wait(0.3);
kasperhangard 0:5e7a5b5b6922 180 grabIn = 0;
kasperhangard 0:5e7a5b5b6922 181 break;
kasperhangard 0:5e7a5b5b6922 182 case 'h':
kasperhangard 0:5e7a5b5b6922 183 // Open the grab
kasperhangard 0:5e7a5b5b6922 184 grabIn = 0;
kasperhangard 0:5e7a5b5b6922 185 grabOut.write(0.5);
kasperhangard 0:5e7a5b5b6922 186 wait(0.3);
kasperhangard 0:5e7a5b5b6922 187 grabOut = 0;
kasperhangard 0:5e7a5b5b6922 188 break;
kasperhangard 0:5e7a5b5b6922 189 case 'x':
kasperhangard 0:5e7a5b5b6922 190 frontSpeed = 0;
kasperhangard 0:5e7a5b5b6922 191 backSpeed = 0;
kasperhangard 0:5e7a5b5b6922 192 rightSpeed = 0;
kasperhangard 0:5e7a5b5b6922 193 leftSpeed = 0;
kasperhangard 0:5e7a5b5b6922 194 leftForward = 0;
kasperhangard 0:5e7a5b5b6922 195 rightForward = 0;
kasperhangard 0:5e7a5b5b6922 196 leftBackward = 0;
kasperhangard 0:5e7a5b5b6922 197 rightBackward = 0;
kasperhangard 0:5e7a5b5b6922 198 grabIn = 0;
kasperhangard 0:5e7a5b5b6922 199 grabOut = 0;
kasperhangard 0:5e7a5b5b6922 200 heightUp = 0;
kasperhangard 0:5e7a5b5b6922 201 heightDown = 0;
kasperhangard 0:5e7a5b5b6922 202 break;
Risifutti 1:06bd2e196518 203 /*
kasperhangard 0:5e7a5b5b6922 204 case 'p':
kasperhangard 0:5e7a5b5b6922 205 if(pilot == false){
kasperhangard 0:5e7a5b5b6922 206 pilot = true;
kasperhangard 0:5e7a5b5b6922 207 while(1){
kasperhangard 0:5e7a5b5b6922 208 selfDrive();
kasperhangard 0:5e7a5b5b6922 209 }
kasperhangard 0:5e7a5b5b6922 210 }
kasperhangard 0:5e7a5b5b6922 211 break;
Risifutti 1:06bd2e196518 212 */
kasperhangard 0:5e7a5b5b6922 213 }
kasperhangard 0:5e7a5b5b6922 214 }
kasperhangard 0:5e7a5b5b6922 215 int main(void) {
kasperhangard 0:5e7a5b5b6922 216 LED = 0.1;
kasperhangard 0:5e7a5b5b6922 217 leftForward = 0;
kasperhangard 0:5e7a5b5b6922 218 rightForward = 0;
kasperhangard 0:5e7a5b5b6922 219 leftBackward = 0;
kasperhangard 0:5e7a5b5b6922 220 rightBackward = 0;
kasperhangard 0:5e7a5b5b6922 221 while (1) {
kasperhangard 0:5e7a5b5b6922 222
kasperhangard 0:5e7a5b5b6922 223 command = BT.getc();
kasperhangard 0:5e7a5b5b6922 224 BT.putc(command);
kasperhangard 0:5e7a5b5b6922 225 motorControl(command);
kasperhangard 0:5e7a5b5b6922 226 }
kasperhangard 0:5e7a5b5b6922 227 }