Radu Chirila / Mbed 2 deprecated TDP3_full

full code not complete yet

Dependencies:   mbed

main.cpp@1:d9fb4f6b30f7, 2019-03-06 (annotated)

Committer:
raduschirila
Date:
Wed Mar 06 16:56:44 2019 +0000
Revision:
1:d9fb4f6b30f7
Parent:
0:3e1d75bd313d 
aaaa

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 1:d9fb4f6b30f7 15 InterruptIn sw1(D10);
raduschirila 1:d9fb4f6b30f7 16
raduschirila 1:d9fb4f6b30f7 17 Ticker cmd;
raduschirila 1:d9fb4f6b30f7 18 char command[256],c;
raduschirila 1:d9fb4f6b30f7 19 int num;
raduschirila 1:d9fb4f6b30f7 20
raduschirila 1:d9fb4f6b30f7 21
raduschirila 0:3e1d75bd313d 22 DigitalOut solenoid(D7);//mosfet gate controlling solenoid
raduschirila 0:3e1d75bd313d 23 PwmOut speed(PTC1);//speed for motors
raduschirila 0:3e1d75bd313d 24 PwmOut speedb(PTA4);
raduschirila 0:3e1d75bd313d 25 DigitalOut fwd1(PTA1);//directions for motors
raduschirila 0:3e1d75bd313d 26 DigitalOut back1(PTA2);
raduschirila 0:3e1d75bd313d 27 DigitalOut fwd2(PTA12);
raduschirila 0:3e1d75bd313d 28 DigitalOut back2(PTD4);
raduschirila 0:3e1d75bd313d 29 DigitalOut leds(PTC8);// the 4 red leds
raduschirila 1:d9fb4f6b30f7 30 int col; // memorise color so it knows which way to go at junction {true for red}
raduschirila 1:d9fb4f6b30f7 31 PwmOut red(PTC9);
raduschirila 1:d9fb4f6b30f7 32 PwmOut blue(PTC8);
raduschirila 1:d9fb4f6b30f7 33 AnalogIn sense(A0);
raduschirila 1:d9fb4f6b30f7 34 Serial pc(USBTX,USBRX);
raduschirila 1:d9fb4f6b30f7 35 float color;
raduschirila 0:3e1d75bd313d 36 float spd1,spd2;//speeds of left and right motors
raduschirila 0:3e1d75bd313d 37
raduschirila 0:3e1d75bd313d 38
raduschirila 0:3e1d75bd313d 39 void solenoid_engage()
raduschirila 0:3e1d75bd313d 40 {
raduschirila 0:3e1d75bd313d 41 solenoid=1;
raduschirila 0:3e1d75bd313d 42 }
raduschirila 0:3e1d75bd313d 43
raduschirila 0:3e1d75bd313d 44 void solenoid_off()
raduschirila 0:3e1d75bd313d 45 {
raduschirila 0:3e1d75bd313d 46 solenoid=0;
raduschirila 0:3e1d75bd313d 47 }
raduschirila 0:3e1d75bd313d 48
raduschirila 0:3e1d75bd313d 49 /*inline void move(spd1,spd2)
raduschirila 0:3e1d75bd313d 50 {
raduschirila 0:3e1d75bd313d 51 {speed1>0 ? fwd1=1;back1=0 : fwd1=0;back1=1};
raduschirila 0:3e1d75bd313d 52 {speed2>0 ? fwd2=1;back2=0 : fwd2=0;back2=1};
raduschirila 0:3e1d75bd313d 53 }
raduschirila 0:3e1d75bd313d 54 */
raduschirila 1:d9fb4f6b30f7 55 inline int detect_color()
raduschirila 1:d9fb4f6b30f7 56 {
raduschirila 1:d9fb4f6b30f7 57 red=0.75;blue=0;
raduschirila 1:d9fb4f6b30f7 58 wait(0.3);
raduschirila 1:d9fb4f6b30f7 59
raduschirila 1:d9fb4f6b30f7 60 color= sense.read();
raduschirila 1:d9fb4f6b30f7 61 red=0;blue=0.75;wait(0.3);
raduschirila 1:d9fb4f6b30f7 62 color=color - sense.read();
raduschirila 1:d9fb4f6b30f7 63 red=0;blue=0;
raduschirila 1:d9fb4f6b30f7 64 if(color >= 0.025 && color < 0.055)
raduschirila 1:d9fb4f6b30f7 65 {
raduschirila 1:d9fb4f6b30f7 66 //pc.printf("RED %.3f\n\n\n\n",color);
raduschirila 1:d9fb4f6b30f7 67 return 0;
raduschirila 1:d9fb4f6b30f7 68 }
raduschirila 1:d9fb4f6b30f7 69 else if( color < 0.025 && color >= 0)
raduschirila 1:d9fb4f6b30f7 70 {
raduschirila 1:d9fb4f6b30f7 71 return 1;
raduschirila 1:d9fb4f6b30f7 72 //pc.printf("BLUE %.3f\n\n\n\n",color);
raduschirila 1:d9fb4f6b30f7 73 }
raduschirila 1:d9fb4f6b30f7 74 else{
raduschirila 1:d9fb4f6b30f7 75 //pc.printf("Unknown Color \n\n\n\n");
raduschirila 1:d9fb4f6b30f7 76 return 2;
raduschirila 1:d9fb4f6b30f7 77 }
raduschirila 1:d9fb4f6b30f7 78 wait(1);
raduschirila 1:d9fb4f6b30f7 79 }
raduschirila 1:d9fb4f6b30f7 80
raduschirila 1:d9fb4f6b30f7 81
raduschirila 1:d9fb4f6b30f7 82
raduschirila 1:d9fb4f6b30f7 83 void command_mode()
raduschirila 0:3e1d75bd313d 84 {
raduschirila 1:d9fb4f6b30f7 85 c=pc.getc();
raduschirila 1:d9fb4f6b30f7 86 if(c=='p')//codename for PID indices
raduschirila 0:3e1d75bd313d 87 {
raduschirila 1:d9fb4f6b30f7 88 pc.printf("PID Coefficients Configuration\n");
raduschirila 1:d9fb4f6b30f7 89 c=pc.getc();
raduschirila 1:d9fb4f6b30f7 90 //c=pc.scanf("%s\n",&command);
raduschirila 1:d9fb4f6b30f7 91 num=(int)c-48;//get numerical value;
raduschirila 1:d9fb4f6b30f7 92 /*switch(num)
raduschirila 1:d9fb4f6b30f7 93 {
raduschirila 1:d9fb4f6b30f7 94 case 1:
raduschirila 1:d9fb4f6b30f7 95 myled=1;
raduschirila 1:d9fb4f6b30f7 96 break;
raduschirila 1:d9fb4f6b30f7 97 case 2:
raduschirila 1:d9fb4f6b30f7 98 myled2=1;
raduschirila 1:d9fb4f6b30f7 99 break;
raduschirila 1:d9fb4f6b30f7 100 case 3:
raduschirila 1:d9fb4f6b30f7 101 myled3=1;
raduschirila 1:d9fb4f6b30f7 102 break;
raduschirila 1:d9fb4f6b30f7 103 default:
raduschirila 1:d9fb4f6b30f7 104 myled=0;myled2=0;myled3=0;break;
raduschirila 1:d9fb4f6b30f7 105 }*/
raduschirila 1:d9fb4f6b30f7 106 pc.printf("Done!\n");
raduschirila 1:d9fb4f6b30f7 107 memset(command,NULL, sizeof(command));
raduschirila 1:d9fb4f6b30f7 108
raduschirila 1:d9fb4f6b30f7 109
raduschirila 1:d9fb4f6b30f7 110 }
raduschirila 1:d9fb4f6b30f7 111 else if(c=='c')//codename for color detection
raduschirila 1:d9fb4f6b30f7 112 {
raduschirila 1:d9fb4f6b30f7 113 col=detect_color();
raduschirila 1:d9fb4f6b30f7 114 pc.printf("%d",col);
raduschirila 1:d9fb4f6b30f7 115 }
raduschirila 1:d9fb4f6b30f7 116 else if(c=='l')//codename for line detection
raduschirila 1:d9fb4f6b30f7 117 {
raduschirila 1:d9fb4f6b30f7 118 //detect line and send stuff over serial -> hex number? could be too big to handle on script side
raduschirila 1:d9fb4f6b30f7 119 pc.printf("112\n");//placeholder code
raduschirila 0:3e1d75bd313d 120 }
raduschirila 0:3e1d75bd313d 121 else
raduschirila 0:3e1d75bd313d 122 {
raduschirila 1:d9fb4f6b30f7 123 pc.printf("Command Unknown.\n");
raduschirila 0:3e1d75bd313d 124 }
raduschirila 0:3e1d75bd313d 125 }
raduschirila 0:3e1d75bd313d 126
raduschirila 1:d9fb4f6b30f7 127
raduschirila 1:d9fb4f6b30f7 128
raduschirila 1:d9fb4f6b30f7 129 int x=0;
raduschirila 1:d9fb4f6b30f7 130 void data(){
raduschirila 1:d9fb4f6b30f7 131 if(x>20) x=0;
raduschirila 1:d9fb4f6b30f7 132 pc.printf("%d\n",x);
raduschirila 1:d9fb4f6b30f7 133 x++;
raduschirila 1:d9fb4f6b30f7 134 if(sw1==1) data();
raduschirila 1:d9fb4f6b30f7 135 else
raduschirila 1:d9fb4f6b30f7 136 {
raduschirila 1:d9fb4f6b30f7 137 pc.printf("%d\n",-1);
raduschirila 1:d9fb4f6b30f7 138 return;
raduschirila 1:d9fb4f6b30f7 139 }
raduschirila 1:d9fb4f6b30f7 140 }
raduschirila 1:d9fb4f6b30f7 141
raduschirila 1:d9fb4f6b30f7 142
raduschirila 1:d9fb4f6b30f7 143
raduschirila 0:3e1d75bd313d 144 int main()
raduschirila 0:3e1d75bd313d 145 {
raduschirila 1:d9fb4f6b30f7 146 sw1.rise(&command_mode);
raduschirila 0:3e1d75bd313d 147 //pwm driver code will go into a function asap
raduschirila 1:d9fb4f6b30f7 148 /*speed.period(0.2f);
raduschirila 0:3e1d75bd313d 149 speedb.period(0.2f);
raduschirila 0:3e1d75bd313d 150
raduschirila 0:3e1d75bd313d 151 speed.write(0.0);speedb.write(0.0);//do not move and scan color
raduschirila 0:3e1d75bd313d 152 float i=0.5,j=0.5;
raduschirila 0:3e1d75bd313d 153
raduschirila 0:3e1d75bd313d 154
raduschirila 0:3e1d75bd313d 155 fwd1=0;back1=1;//fwd
raduschirila 0:3e1d75bd313d 156 fwd2=1;back2=0;
raduschirila 0:3e1d75bd313d 157 speed.write(i);
raduschirila 0:3e1d75bd313d 158 speedb.write(j);
raduschirila 0:3e1d75bd313d 159 wait(2);
raduschirila 0:3e1d75bd313d 160
raduschirila 0:3e1d75bd313d 161
raduschirila 0:3e1d75bd313d 162 fwd1=0;back1=1;//back
raduschirila 0:3e1d75bd313d 163 fwd2=0;back2=1;
raduschirila 0:3e1d75bd313d 164 speed.write(0);
raduschirila 0:3e1d75bd313d 165 speedb.write(0);
raduschirila 1:d9fb4f6b30f7 166 wait(2);*/
raduschirila 0:3e1d75bd313d 167
raduschirila 0:3e1d75bd313d 168 }