TDP3 / Mbed 2 deprecated TDP3_full3

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@1:4a4e697b10d2, 2019-03-18 (annotated)

Committer:
raduschirila
Date:
Mon Mar 18 16:55:28 2019 +0000
Revision:
1:4a4e697b10d2
Parent:
0:5121458888b5 
pdd not working;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
raduschirila 0:5121458888b5 1 #include "mbed.h"
raduschirila 0:5121458888b5 2
raduschirila 0:5121458888b5 3 DigitalOut solenoid(PTC9);//mosfet gate controlling solenoid
raduschirila 0:5121458888b5 4 PwmOut speed(PTC1);//speed for motors
raduschirila 0:5121458888b5 5 PwmOut speedb(PTA4);
raduschirila 0:5121458888b5 6 DigitalOut fwd1(PTA1);//directions for motors
raduschirila 0:5121458888b5 7 DigitalOut back1(PTA2);
raduschirila 0:5121458888b5 8 DigitalOut fwd2(PTA12);
raduschirila 0:5121458888b5 9 DigitalOut back2(PTD4);
raduschirila 0:5121458888b5 10 PwmOut red(PTA5);
raduschirila 0:5121458888b5 11 PwmOut blue(PTC8);
raduschirila 0:5121458888b5 12 AnalogIn sense(A0);
raduschirila 0:5121458888b5 13 DigitalOut test_red(LED_RED);
raduschirila 0:5121458888b5 14 DigitalOut test_blue(LED_BLUE);
raduschirila 0:5121458888b5 15 Serial pc(PTE0,PTE1); // tx, rx
raduschirila 0:5121458888b5 16 BusIn line(PTC3,PTC0,PTC7,PTC5,PTC4,PTC6,PTC10); //Line Board input
raduschirila 1:4a4e697b10d2 17 Ticker timed;
raduschirila 0:5121458888b5 18 char command[256],c;
raduschirila 0:5121458888b5 19 int num;
raduschirila 1:4a4e697b10d2 20 int col;
raduschirila 0:5121458888b5 21 float color;
raduschirila 1:4a4e697b10d2 22 bool detected=false;
raduschirila 0:5121458888b5 23 // Minimum and maximum motor speeds
raduschirila 1:4a4e697b10d2 24 #define MAX 0.5
raduschirila 0:5121458888b5 25 // PID terms
raduschirila 1:4a4e697b10d2 26 #define P_TERM 30
raduschirila 1:4a4e697b10d2 27 #define DD_TERM 2
raduschirila 0:5121458888b5 28 #define D_TERM 76
raduschirila 0:5121458888b5 29 #define k 0.1
raduschirila 1:4a4e697b10d2 30
raduschirila 1:4a4e697b10d2 31 inline void junction();
raduschirila 1:4a4e697b10d2 32 inline void color_detect();
raduschirila 0:5121458888b5 33 inline float get_position()//-3 to 3 based on the sensor position
raduschirila 0:5121458888b5 34 {
raduschirila 0:5121458888b5 35 switch(line)//rates are wrong CHANGE ASAP
raduschirila 0:5121458888b5 36 {
raduschirila 0:5121458888b5 37 case 0x01:
raduschirila 0:5121458888b5 38 return -1.0;
raduschirila 0:5121458888b5 39 case 0x02:
raduschirila 0:5121458888b5 40 return -0.9;
raduschirila 0:5121458888b5 41 case 0x04:
raduschirila 0:5121458888b5 42 return -0.3;
raduschirila 0:5121458888b5 43 case 0x08:
raduschirila 0:5121458888b5 44 return 0;
raduschirila 0:5121458888b5 45 case 0x10:
raduschirila 0:5121458888b5 46 return 0.3;
raduschirila 0:5121458888b5 47 case 0x20:
raduschirila 0:5121458888b5 48 return 0.9;
raduschirila 0:5121458888b5 49 case 0x40:
raduschirila 0:5121458888b5 50 return 1.0;
raduschirila 1:4a4e697b10d2 51 case 0x14:
raduschirila 1:4a4e697b10d2 52 junction();
raduschirila 1:4a4e697b10d2 53 detected=false;
raduschirila 1:4a4e697b10d2 54 timed.attach(&color_detect,0.2);
raduschirila 1:4a4e697b10d2 55 break;
raduschirila 1:4a4e697b10d2 56 case 0x1C:
raduschirila 1:4a4e697b10d2 57 junction();
raduschirila 1:4a4e697b10d2 58 detected=false;
raduschirila 1:4a4e697b10d2 59 timed.attach(&color_detect,0.2);
raduschirila 1:4a4e697b10d2 60 break;
raduschirila 1:4a4e697b10d2 61 case 0x28:
raduschirila 1:4a4e697b10d2 62 junction();
raduschirila 1:4a4e697b10d2 63 detected=false;
raduschirila 1:4a4e697b10d2 64 timed.attach(&color_detect,0.2);
raduschirila 1:4a4e697b10d2 65 break;
raduschirila 1:4a4e697b10d2 66 case 0x50:
raduschirila 1:4a4e697b10d2 67 junction();
raduschirila 1:4a4e697b10d2 68 detected=false;
raduschirila 1:4a4e697b10d2 69 timed.attach(&color_detect,0.2);
raduschirila 1:4a4e697b10d2 70 break;
raduschirila 1:4a4e697b10d2 71 case 0x05:
raduschirila 1:4a4e697b10d2 72 junction();
raduschirila 1:4a4e697b10d2 73 detected=false;
raduschirila 1:4a4e697b10d2 74 timed.attach(&color_detect,0.2);
raduschirila 1:4a4e697b10d2 75 break;
raduschirila 1:4a4e697b10d2 76 case 0x0A:
raduschirila 1:4a4e697b10d2 77 junction();
raduschirila 1:4a4e697b10d2 78 detected=false;
raduschirila 1:4a4e697b10d2 79 timed.attach(&color_detect,0.2);
raduschirila 1:4a4e697b10d2 80 break;
raduschirila 1:4a4e697b10d2 81 case 0x24:
raduschirila 1:4a4e697b10d2 82 junction();
raduschirila 1:4a4e697b10d2 83 detected=false;
raduschirila 1:4a4e697b10d2 84 timed.attach(&color_detect,0.2);
raduschirila 1:4a4e697b10d2 85 break;
raduschirila 1:4a4e697b10d2 86 case 0x12:
raduschirila 1:4a4e697b10d2 87 junction();
raduschirila 1:4a4e697b10d2 88 detected=false;
raduschirila 1:4a4e697b10d2 89 timed.attach(&color_detect,0.2);
raduschirila 1:4a4e697b10d2 90 break;
raduschirila 1:4a4e697b10d2 91 case 0x41:
raduschirila 1:4a4e697b10d2 92 junction();
raduschirila 1:4a4e697b10d2 93 detected=false;
raduschirila 1:4a4e697b10d2 94 timed.attach(&color_detect,0.2);
raduschirila 1:4a4e697b10d2 95 break;
raduschirila 1:4a4e697b10d2 96
raduschirila 0:5121458888b5 97 }
raduschirila 0:5121458888b5 98 }
raduschirila 0:5121458888b5 99
raduschirila 0:5121458888b5 100 void drive(float left,float right)
raduschirila 0:5121458888b5 101 {
raduschirila 0:5121458888b5 102 if(left==right)
raduschirila 0:5121458888b5 103 {
raduschirila 0:5121458888b5 104 fwd1=1;back1=0;
raduschirila 0:5121458888b5 105 fwd2=1;back2=0;
raduschirila 0:5121458888b5 106 }
raduschirila 0:5121458888b5 107 if(left>right)
raduschirila 0:5121458888b5 108 {
raduschirila 0:5121458888b5 109 fwd1=1;back1=0;
raduschirila 0:5121458888b5 110 fwd2=0;back2=1;
raduschirila 0:5121458888b5 111 }
raduschirila 0:5121458888b5 112 if(left<right)
raduschirila 0:5121458888b5 113 {
raduschirila 0:5121458888b5 114 fwd1=0;back1=1;
raduschirila 0:5121458888b5 115 fwd2=1;back2=0;
raduschirila 0:5121458888b5 116 }
raduschirila 0:5121458888b5 117 speed.write(left);
raduschirila 0:5121458888b5 118 speedb.write(right);
raduschirila 0:5121458888b5 119
raduschirila 0:5121458888b5 120 }
raduschirila 1:4a4e697b10d2 121
raduschirila 1:4a4e697b10d2 122 inline void junction()//TO GET OUT OF JUNCTION
raduschirila 1:4a4e697b10d2 123 {
raduschirila 1:4a4e697b10d2 124 if(col== 1)
raduschirila 1:4a4e697b10d2 125 {
raduschirila 1:4a4e697b10d2 126 //turn left
raduschirila 1:4a4e697b10d2 127 drive(0.5,0.1);wait(0.1);
raduschirila 1:4a4e697b10d2 128 }
raduschirila 1:4a4e697b10d2 129 else
raduschirila 1:4a4e697b10d2 130 {
raduschirila 1:4a4e697b10d2 131 drive( 0.1,0.5);wait(0.1);
raduschirila 1:4a4e697b10d2 132 //turn right
raduschirila 1:4a4e697b10d2 133 }
raduschirila 1:4a4e697b10d2 134 }
raduschirila 1:4a4e697b10d2 135 inline void solenoid_actuate()
raduschirila 1:4a4e697b10d2 136 {
raduschirila 1:4a4e697b10d2 137 solenoid=!solenoid;
raduschirila 1:4a4e697b10d2 138 }
raduschirila 1:4a4e697b10d2 139 inline void color_detect()
raduschirila 1:4a4e697b10d2 140 {
raduschirila 1:4a4e697b10d2 141 //for timed functions
raduschirila 1:4a4e697b10d2 142 if(detected)
raduschirila 1:4a4e697b10d2 143 {
raduschirila 1:4a4e697b10d2 144 timed.detach();
raduschirila 1:4a4e697b10d2 145 return;
raduschirila 1:4a4e697b10d2 146 }
raduschirila 1:4a4e697b10d2 147 else{
raduschirila 1:4a4e697b10d2 148 red=0.8;blue=0;
raduschirila 1:4a4e697b10d2 149
raduschirila 1:4a4e697b10d2 150 wait(0.1);
raduschirila 1:4a4e697b10d2 151
raduschirila 1:4a4e697b10d2 152 color= sense.read();
raduschirila 1:4a4e697b10d2 153
raduschirila 1:4a4e697b10d2 154 red=0;blue=0.8;wait(0.1);
raduschirila 1:4a4e697b10d2 155
raduschirila 1:4a4e697b10d2 156 color=color - sense.read();
raduschirila 1:4a4e697b10d2 157
raduschirila 1:4a4e697b10d2 158 red=0;blue=0;
raduschirila 1:4a4e697b10d2 159
raduschirila 1:4a4e697b10d2 160 if(color >= 0.033 && color < 0.045)
raduschirila 1:4a4e697b10d2 161
raduschirila 1:4a4e697b10d2 162 {
raduschirila 1:4a4e697b10d2 163 detected=true;
raduschirila 1:4a4e697b10d2 164 solenoid_actuate();
raduschirila 1:4a4e697b10d2 165 test_red=0;test_blue=1;
raduschirila 1:4a4e697b10d2 166 col =1;
raduschirila 1:4a4e697b10d2 167
raduschirila 1:4a4e697b10d2 168 }
raduschirila 1:4a4e697b10d2 169
raduschirila 1:4a4e697b10d2 170 else if( color <= 0.020 && color >= 0)
raduschirila 1:4a4e697b10d2 171
raduschirila 1:4a4e697b10d2 172 {
raduschirila 1:4a4e697b10d2 173 detected=true;
raduschirila 1:4a4e697b10d2 174 test_blue=0;test_red=1;
raduschirila 1:4a4e697b10d2 175 solenoid_actuate();
raduschirila 1:4a4e697b10d2 176 col=0;
raduschirila 1:4a4e697b10d2 177
raduschirila 1:4a4e697b10d2 178 }
raduschirila 1:4a4e697b10d2 179 }
raduschirila 1:4a4e697b10d2 180 }
raduschirila 0:5121458888b5 181
raduschirila 0:5121458888b5 182 inline int detect_color()
raduschirila 0:5121458888b5 183 {
raduschirila 0:5121458888b5 184 red=0.75;blue=0;
raduschirila 0:5121458888b5 185
raduschirila 1:4a4e697b10d2 186 wait(0.08);
raduschirila 0:5121458888b5 187
raduschirila 0:5121458888b5 188
raduschirila 0:5121458888b5 189
raduschirila 0:5121458888b5 190 color= sense.read();
raduschirila 0:5121458888b5 191
raduschirila 1:4a4e697b10d2 192 red=0;blue=0.75;wait(0.08);
raduschirila 0:5121458888b5 193
raduschirila 0:5121458888b5 194 color=color - sense.read();
raduschirila 0:5121458888b5 195
raduschirila 0:5121458888b5 196 red=0;blue=0;
raduschirila 0:5121458888b5 197
raduschirila 1:4a4e697b10d2 198 if(color >= 0.035 && color < 0.045)
raduschirila 0:5121458888b5 199
raduschirila 0:5121458888b5 200 {
raduschirila 0:5121458888b5 201
raduschirila 1:4a4e697b10d2 202 pc.printf("RED %.3f\n\n\n\n",color);
raduschirila 0:5121458888b5 203 test_red=0;test_blue=1;
raduschirila 0:5121458888b5 204 return 0;
raduschirila 0:5121458888b5 205
raduschirila 0:5121458888b5 206 }
raduschirila 0:5121458888b5 207
raduschirila 1:4a4e697b10d2 208 else if( color < 0.02 && color >= 0)
raduschirila 0:5121458888b5 209
raduschirila 0:5121458888b5 210 {
raduschirila 0:5121458888b5 211 test_blue=0;test_red=1;
raduschirila 0:5121458888b5 212 return 1;
raduschirila 0:5121458888b5 213
raduschirila 1:4a4e697b10d2 214 pc.printf("BLUE %.3f\n\n\n\n",color);
raduschirila 0:5121458888b5 215
raduschirila 0:5121458888b5 216 }
raduschirila 0:5121458888b5 217
raduschirila 0:5121458888b5 218 else{
raduschirila 0:5121458888b5 219
raduschirila 0:5121458888b5 220 //pc.printf("Unknown Color \n\n\n\n");
raduschirila 0:5121458888b5 221
raduschirila 0:5121458888b5 222 return 2;
raduschirila 0:5121458888b5 223
raduschirila 0:5121458888b5 224 }
raduschirila 0:5121458888b5 225
raduschirila 0:5121458888b5 226 //wait(1);
raduschirila 0:5121458888b5 227
raduschirila 0:5121458888b5 228 }
raduschirila 0:5121458888b5 229
raduschirila 0:5121458888b5 230 void command_mode()
raduschirila 0:5121458888b5 231 {
raduschirila 0:5121458888b5 232 pc.printf("\n");
raduschirila 0:5121458888b5 233 while(1)
raduschirila 0:5121458888b5 234 {
raduschirila 0:5121458888b5 235 c=pc.getc();
raduschirila 0:5121458888b5 236
raduschirila 0:5121458888b5 237 if(c=='p')//codename for PID indices
raduschirila 0:5121458888b5 238
raduschirila 0:5121458888b5 239 {
raduschirila 0:5121458888b5 240
raduschirila 0:5121458888b5 241 pc.printf("PID Coefficients Configuration\n");
raduschirila 0:5121458888b5 242 //pc.printf("kp %f, ki %f, kd %f",kp,ki,kd);
raduschirila 0:5121458888b5 243 c=pc.getc();
raduschirila 0:5121458888b5 244
raduschirila 0:5121458888b5 245 num=(int)c-48;//get numerical value;
raduschirila 0:5121458888b5 246
raduschirila 0:5121458888b5 247 switch(num)
raduschirila 0:5121458888b5 248 {
raduschirila 0:5121458888b5 249
raduschirila 0:5121458888b5 250 case 1:
raduschirila 0:5121458888b5 251
raduschirila 0:5121458888b5 252
raduschirila 0:5121458888b5 253 break;
raduschirila 0:5121458888b5 254
raduschirila 0:5121458888b5 255 case 2:
raduschirila 0:5121458888b5 256
raduschirila 0:5121458888b5 257
raduschirila 0:5121458888b5 258 break;
raduschirila 0:5121458888b5 259
raduschirila 0:5121458888b5 260 case 3:
raduschirila 0:5121458888b5 261
raduschirila 0:5121458888b5 262
raduschirila 0:5121458888b5 263
raduschirila 0:5121458888b5 264 break;
raduschirila 0:5121458888b5 265
raduschirila 0:5121458888b5 266 default:
raduschirila 0:5121458888b5 267
raduschirila 0:5121458888b5 268 break;
raduschirila 0:5121458888b5 269
raduschirila 0:5121458888b5 270 }
raduschirila 0:5121458888b5 271
raduschirila 0:5121458888b5 272 pc.printf("Done!\n");
raduschirila 0:5121458888b5 273
raduschirila 0:5121458888b5 274 memset(command,NULL, sizeof(command));
raduschirila 0:5121458888b5 275
raduschirila 0:5121458888b5 276
raduschirila 0:5121458888b5 277 }
raduschirila 0:5121458888b5 278
raduschirila 0:5121458888b5 279 else if(c=='c')//codename for color detection
raduschirila 0:5121458888b5 280
raduschirila 0:5121458888b5 281 {
raduschirila 0:5121458888b5 282 int col;
raduschirila 0:5121458888b5 283 col=detect_color();
raduschirila 0:5121458888b5 284
raduschirila 0:5121458888b5 285 pc.printf("%d\n",col);
raduschirila 0:5121458888b5 286
raduschirila 0:5121458888b5 287 }
raduschirila 0:5121458888b5 288
raduschirila 0:5121458888b5 289 else if(c=='l')//codename for line detection
raduschirila 0:5121458888b5 290
raduschirila 0:5121458888b5 291 {
raduschirila 0:5121458888b5 292 pc.printf("%f\n",get_position());//placeholder code
raduschirila 0:5121458888b5 293
raduschirila 0:5121458888b5 294 }
raduschirila 0:5121458888b5 295 else if(c=='s')
raduschirila 0:5121458888b5 296 {
raduschirila 0:5121458888b5 297 if((int)solenoid.read() == 1)
raduschirila 0:5121458888b5 298 {
raduschirila 0:5121458888b5 299 solenoid=0;
raduschirila 0:5121458888b5 300 pc.printf("Solenoid disengaged\n");
raduschirila 0:5121458888b5 301 }
raduschirila 0:5121458888b5 302 else if ((int)solenoid.read() ==0)
raduschirila 0:5121458888b5 303 {
raduschirila 0:5121458888b5 304 solenoid = 1;
raduschirila 0:5121458888b5 305 pc.printf("Solenoid engaged\n");
raduschirila 0:5121458888b5 306 }
raduschirila 0:5121458888b5 307
raduschirila 0:5121458888b5 308 }
raduschirila 0:5121458888b5 309
raduschirila 0:5121458888b5 310 else
raduschirila 0:5121458888b5 311 {
raduschirila 0:5121458888b5 312 pc.printf("Command Unknown.\n");
raduschirila 0:5121458888b5 313 }
raduschirila 0:5121458888b5 314 }
raduschirila 0:5121458888b5 315 }
raduschirila 0:5121458888b5 316
raduschirila 0:5121458888b5 317
raduschirila 0:5121458888b5 318 int main()
raduschirila 0:5121458888b5 319 {
raduschirila 1:4a4e697b10d2 320
raduschirila 0:5121458888b5 321 wait(2);
raduschirila 0:5121458888b5 322 if(pc.readable())
raduschirila 0:5121458888b5 323 {
raduschirila 1:4a4e697b10d2 324 if((char)pc.getc()=='z'){
raduschirila 1:4a4e697b10d2 325 timed.detach();
raduschirila 0:5121458888b5 326 command_mode();
raduschirila 1:4a4e697b10d2 327 }
raduschirila 0:5121458888b5 328 }
raduschirila 0:5121458888b5 329 else
raduschirila 0:5121458888b5 330 {
raduschirila 1:4a4e697b10d2 331
raduschirila 0:5121458888b5 332 wait(2.0);
raduschirila 1:4a4e697b10d2 333 solenoid=0;
raduschirila 0:5121458888b5 334 float right;
raduschirila 0:5121458888b5 335 float left;
raduschirila 0:5121458888b5 336 float current_pos_of_line = 0.0;
raduschirila 0:5121458888b5 337 float previous_pos_of_line = 0.0;
raduschirila 0:5121458888b5 338 float previous_derivative=0;
raduschirila 0:5121458888b5 339 float derivative,proportional,DoD = 0;
raduschirila 0:5121458888b5 340 float power;
raduschirila 0:5121458888b5 341 float speed = MAX;
raduschirila 1:4a4e697b10d2 342 timed.attach(&color_detect,0.25);
raduschirila 0:5121458888b5 343 while (1) {
raduschirila 0:5121458888b5 344 // Get the position of the line.
raduschirila 0:5121458888b5 345 current_pos_of_line = get_position();
raduschirila 0:5121458888b5 346 proportional = current_pos_of_line;
raduschirila 0:5121458888b5 347
raduschirila 0:5121458888b5 348 // Compute the derivative
raduschirila 0:5121458888b5 349 derivative = current_pos_of_line - previous_pos_of_line;
raduschirila 0:5121458888b5 350
raduschirila 0:5121458888b5 351 // Compute the DoD
raduschirila 0:5121458888b5 352 DoD = derivative - previous_derivative;
raduschirila 0:5121458888b5 353
raduschirila 0:5121458888b5 354 // Remember the last position.
raduschirila 0:5121458888b5 355 previous_pos_of_line = current_pos_of_line;
raduschirila 0:5121458888b5 356 previous_derivative = derivative;
raduschirila 0:5121458888b5 357 // Compute the power
raduschirila 0:5121458888b5 358 power = k*(proportional * (P_TERM) ) + (DoD*(DD_TERM)) + (derivative*(D_TERM)) ;
raduschirila 0:5121458888b5 359
raduschirila 0:5121458888b5 360
raduschirila 0:5121458888b5 361
raduschirila 0:5121458888b5 362
raduschirila 0:5121458888b5 363 // Compute new speeds
raduschirila 0:5121458888b5 364 right = speed+power;
raduschirila 0:5121458888b5 365 left = speed-power;
raduschirila 0:5121458888b5 366
raduschirila 0:5121458888b5 367 // set speed
raduschirila 0:5121458888b5 368 drive(left,right);
raduschirila 0:5121458888b5 369 }
raduschirila 0:5121458888b5 370 }
raduschirila 0:5121458888b5 371 }