Radu Chirila / Mbed 2 deprecated TDP3_full

full code not complete yet

Dependencies:   mbed

main.cpp@0:3e1d75bd313d, 2019-02-12 (annotated)

Committer:
raduschirila
Date:
Tue Feb 12 17:52:51 2019 +0000
Revision:
0:3e1d75bd313d
Child:
1:d9fb4f6b30f7
dkdkdkdkdkdkdkdkd;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
raduschirila 0:3e1d75bd313d 1 /*
raduschirila 0:3e1d75bd313d 2 TODO:
raduschirila 0:3e1d75bd313d 3 Line following, gaussian filter,
raduschirila 0:3e1d75bd313d 4 Color det integration
raduschirila 0:3e1d75bd313d 5 PID algorithm integration
raduschirila 0:3e1d75bd313d 6 Line following integration
raduschirila 0:3e1d75bd313d 7 Calibration????
raduschirila 0:3e1d75bd313d 8
raduschirila 0:3e1d75bd313d 9 */
raduschirila 0:3e1d75bd313d 10
raduschirila 0:3e1d75bd313d 11
raduschirila 0:3e1d75bd313d 12
raduschirila 0:3e1d75bd313d 13 #include "mbed.h"
raduschirila 0:3e1d75bd313d 14
raduschirila 0:3e1d75bd313d 15 DigitalOut solenoid(D7);//mosfet gate controlling solenoid
raduschirila 0:3e1d75bd313d 16 PwmOut speed(PTC1);//speed for motors
raduschirila 0:3e1d75bd313d 17 PwmOut speedb(PTA4);
raduschirila 0:3e1d75bd313d 18 DigitalOut fwd1(PTA1);//directions for motors
raduschirila 0:3e1d75bd313d 19 DigitalOut back1(PTA2);
raduschirila 0:3e1d75bd313d 20 DigitalOut fwd2(PTA12);
raduschirila 0:3e1d75bd313d 21 DigitalOut back2(PTD4);
raduschirila 0:3e1d75bd313d 22 DigitalOut leds(PTC8);// the 4 red leds
raduschirila 0:3e1d75bd313d 23 bool c; // memorise color so it knows which way to go at junction {true for red}
raduschirila 0:3e1d75bd313d 24 float white_reference,color;
raduschirila 0:3e1d75bd313d 25 float spd1,spd2;//speeds of left and right motors
raduschirila 0:3e1d75bd313d 26
raduschirila 0:3e1d75bd313d 27
raduschirila 0:3e1d75bd313d 28 void solenoid_engage()
raduschirila 0:3e1d75bd313d 29 {
raduschirila 0:3e1d75bd313d 30 solenoid=1;
raduschirila 0:3e1d75bd313d 31 }
raduschirila 0:3e1d75bd313d 32
raduschirila 0:3e1d75bd313d 33 void solenoid_off()
raduschirila 0:3e1d75bd313d 34 {
raduschirila 0:3e1d75bd313d 35 solenoid=0;
raduschirila 0:3e1d75bd313d 36 }
raduschirila 0:3e1d75bd313d 37
raduschirila 0:3e1d75bd313d 38 /*inline void move(spd1,spd2)
raduschirila 0:3e1d75bd313d 39 {
raduschirila 0:3e1d75bd313d 40 {speed1>0 ? fwd1=1;back1=0 : fwd1=0;back1=1};
raduschirila 0:3e1d75bd313d 41 {speed2>0 ? fwd2=1;back2=0 : fwd2=0;back2=1};
raduschirila 0:3e1d75bd313d 42 }
raduschirila 0:3e1d75bd313d 43 */
raduschirila 0:3e1d75bd313d 44 inline bool detect_color(int stage)
raduschirila 0:3e1d75bd313d 45 {
raduschirila 0:3e1d75bd313d 46 leds=1;//turn on the 4 red leds
raduschirila 0:3e1d75bd313d 47 if(stage==0)
raduschirila 0:3e1d75bd313d 48 {
raduschirila 0:3e1d75bd313d 49 white_reference=colorsensor.read();//global variable no prob
raduschirila 0:3e1d75bd313d 50 }
raduschirila 0:3e1d75bd313d 51 else
raduschirila 0:3e1d75bd313d 52 {
raduschirila 0:3e1d75bd313d 53 color=colorsensor.read();
raduschirila 0:3e1d75bd313d 54 return (color> white_reference (+/-) threshold ? true:false); //true means red, false means blue
raduschirila 0:3e1d75bd313d 55 }
raduschirila 0:3e1d75bd313d 56 }
raduschirila 0:3e1d75bd313d 57
raduschirila 0:3e1d75bd313d 58 int main()
raduschirila 0:3e1d75bd313d 59 {
raduschirila 0:3e1d75bd313d 60 //pwm driver code will go into a function asap
raduschirila 0:3e1d75bd313d 61 speed.period(0.2f);
raduschirila 0:3e1d75bd313d 62 speedb.period(0.2f);
raduschirila 0:3e1d75bd313d 63
raduschirila 0:3e1d75bd313d 64 speed.write(0.0);speedb.write(0.0);//do not move and scan color
raduschirila 0:3e1d75bd313d 65 float i=0.5,j=0.5;
raduschirila 0:3e1d75bd313d 66
raduschirila 0:3e1d75bd313d 67
raduschirila 0:3e1d75bd313d 68 fwd1=0;back1=1;//fwd
raduschirila 0:3e1d75bd313d 69 fwd2=1;back2=0;
raduschirila 0:3e1d75bd313d 70 speed.write(i);
raduschirila 0:3e1d75bd313d 71 speedb.write(j);
raduschirila 0:3e1d75bd313d 72 wait(2);
raduschirila 0:3e1d75bd313d 73
raduschirila 0:3e1d75bd313d 74
raduschirila 0:3e1d75bd313d 75 fwd1=0;back1=1;//back
raduschirila 0:3e1d75bd313d 76 fwd2=0;back2=1;
raduschirila 0:3e1d75bd313d 77 speed.write(0);
raduschirila 0:3e1d75bd313d 78 speedb.write(0);
raduschirila 0:3e1d75bd313d 79 wait(2);
raduschirila 0:3e1d75bd313d 80
raduschirila 0:3e1d75bd313d 81 }