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@0:5121458888b5, 2019-03-17 (annotated)

Committer:
raduschirila
Date:
Sun Mar 17 21:21:17 2019 +0000
Revision:
0:5121458888b5
Child:
1:4a4e697b10d2
final one this time;

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 0:5121458888b5 17 char command[256],c;
raduschirila 0:5121458888b5 18 int num;
raduschirila 0:5121458888b5 19 float color;
raduschirila 0:5121458888b5 20 // Minimum and maximum motor speeds
raduschirila 0:5121458888b5 21 #define MAX 0.9
raduschirila 0:5121458888b5 22
raduschirila 0:5121458888b5 23
raduschirila 0:5121458888b5 24 // PID terms
raduschirila 0:5121458888b5 25 #define P_TERM 30
raduschirila 0:5121458888b5 26 #define DD_TERM 0
raduschirila 0:5121458888b5 27 #define D_TERM 76
raduschirila 0:5121458888b5 28 #define k 0.1
raduschirila 0:5121458888b5 29 inline float get_position()//-3 to 3 based on the sensor position
raduschirila 0:5121458888b5 30 {
raduschirila 0:5121458888b5 31 switch(line)//rates are wrong CHANGE ASAP
raduschirila 0:5121458888b5 32 {
raduschirila 0:5121458888b5 33 case 0x01:
raduschirila 0:5121458888b5 34 return -1.0;
raduschirila 0:5121458888b5 35 case 0x02:
raduschirila 0:5121458888b5 36 return -0.9;
raduschirila 0:5121458888b5 37 case 0x04:
raduschirila 0:5121458888b5 38 return -0.3;
raduschirila 0:5121458888b5 39 case 0x08:
raduschirila 0:5121458888b5 40 return 0;
raduschirila 0:5121458888b5 41 case 0x10:
raduschirila 0:5121458888b5 42 return 0.3;
raduschirila 0:5121458888b5 43 case 0x20:
raduschirila 0:5121458888b5 44 return 0.9;
raduschirila 0:5121458888b5 45 case 0x40:
raduschirila 0:5121458888b5 46 return 1.0;
raduschirila 0:5121458888b5 47 }
raduschirila 0:5121458888b5 48 }
raduschirila 0:5121458888b5 49
raduschirila 0:5121458888b5 50 void drive(float left,float right)
raduschirila 0:5121458888b5 51 {
raduschirila 0:5121458888b5 52 if(left==right)
raduschirila 0:5121458888b5 53 {
raduschirila 0:5121458888b5 54 fwd1=1;back1=0;
raduschirila 0:5121458888b5 55 fwd2=1;back2=0;
raduschirila 0:5121458888b5 56 }
raduschirila 0:5121458888b5 57 if(left>right)
raduschirila 0:5121458888b5 58 {
raduschirila 0:5121458888b5 59 fwd1=1;back1=0;
raduschirila 0:5121458888b5 60 fwd2=0;back2=1;
raduschirila 0:5121458888b5 61 }
raduschirila 0:5121458888b5 62 if(left<right)
raduschirila 0:5121458888b5 63 {
raduschirila 0:5121458888b5 64 fwd1=0;back1=1;
raduschirila 0:5121458888b5 65 fwd2=1;back2=0;
raduschirila 0:5121458888b5 66 }
raduschirila 0:5121458888b5 67 speed.write(left);
raduschirila 0:5121458888b5 68 speedb.write(right);
raduschirila 0:5121458888b5 69
raduschirila 0:5121458888b5 70 }
raduschirila 0:5121458888b5 71
raduschirila 0:5121458888b5 72
raduschirila 0:5121458888b5 73 inline int detect_color()
raduschirila 0:5121458888b5 74
raduschirila 0:5121458888b5 75 {
raduschirila 0:5121458888b5 76 solenoid=1;
raduschirila 0:5121458888b5 77 red=0.75;blue=0;
raduschirila 0:5121458888b5 78
raduschirila 0:5121458888b5 79 wait(0.01);
raduschirila 0:5121458888b5 80
raduschirila 0:5121458888b5 81
raduschirila 0:5121458888b5 82
raduschirila 0:5121458888b5 83 color= sense.read();
raduschirila 0:5121458888b5 84
raduschirila 0:5121458888b5 85 red=0;blue=0.75;wait(0.01);
raduschirila 0:5121458888b5 86
raduschirila 0:5121458888b5 87 color=color - sense.read();
raduschirila 0:5121458888b5 88
raduschirila 0:5121458888b5 89 red=0;blue=0;
raduschirila 0:5121458888b5 90
raduschirila 0:5121458888b5 91 if(color >= 0.025 && color < 0.055)
raduschirila 0:5121458888b5 92
raduschirila 0:5121458888b5 93 {
raduschirila 0:5121458888b5 94
raduschirila 0:5121458888b5 95 //pc.printf("RED %.3f\n\n\n\n",color);
raduschirila 0:5121458888b5 96 test_red=0;test_blue=1;
raduschirila 0:5121458888b5 97 return 0;
raduschirila 0:5121458888b5 98
raduschirila 0:5121458888b5 99 }
raduschirila 0:5121458888b5 100
raduschirila 0:5121458888b5 101 else if( color < 0.025 && color >= 0)
raduschirila 0:5121458888b5 102
raduschirila 0:5121458888b5 103 {
raduschirila 0:5121458888b5 104 test_blue=0;test_red=1;
raduschirila 0:5121458888b5 105 return 1;
raduschirila 0:5121458888b5 106
raduschirila 0:5121458888b5 107 //pc.printf("BLUE %.3f\n\n\n\n",color);
raduschirila 0:5121458888b5 108
raduschirila 0:5121458888b5 109 }
raduschirila 0:5121458888b5 110
raduschirila 0:5121458888b5 111 else{
raduschirila 0:5121458888b5 112
raduschirila 0:5121458888b5 113 //pc.printf("Unknown Color \n\n\n\n");
raduschirila 0:5121458888b5 114
raduschirila 0:5121458888b5 115 return 2;
raduschirila 0:5121458888b5 116
raduschirila 0:5121458888b5 117 }
raduschirila 0:5121458888b5 118
raduschirila 0:5121458888b5 119 //wait(1);
raduschirila 0:5121458888b5 120
raduschirila 0:5121458888b5 121 }
raduschirila 0:5121458888b5 122
raduschirila 0:5121458888b5 123 void command_mode()
raduschirila 0:5121458888b5 124 {
raduschirila 0:5121458888b5 125 pc.printf("\n");
raduschirila 0:5121458888b5 126 while(1)
raduschirila 0:5121458888b5 127 {
raduschirila 0:5121458888b5 128 c=pc.getc();
raduschirila 0:5121458888b5 129
raduschirila 0:5121458888b5 130 if(c=='p')//codename for PID indices
raduschirila 0:5121458888b5 131
raduschirila 0:5121458888b5 132 {
raduschirila 0:5121458888b5 133
raduschirila 0:5121458888b5 134 pc.printf("PID Coefficients Configuration\n");
raduschirila 0:5121458888b5 135 //pc.printf("kp %f, ki %f, kd %f",kp,ki,kd);
raduschirila 0:5121458888b5 136 c=pc.getc();
raduschirila 0:5121458888b5 137
raduschirila 0:5121458888b5 138 num=(int)c-48;//get numerical value;
raduschirila 0:5121458888b5 139
raduschirila 0:5121458888b5 140 switch(num)
raduschirila 0:5121458888b5 141 {
raduschirila 0:5121458888b5 142
raduschirila 0:5121458888b5 143 case 1:
raduschirila 0:5121458888b5 144
raduschirila 0:5121458888b5 145
raduschirila 0:5121458888b5 146 break;
raduschirila 0:5121458888b5 147
raduschirila 0:5121458888b5 148 case 2:
raduschirila 0:5121458888b5 149
raduschirila 0:5121458888b5 150
raduschirila 0:5121458888b5 151 break;
raduschirila 0:5121458888b5 152
raduschirila 0:5121458888b5 153 case 3:
raduschirila 0:5121458888b5 154
raduschirila 0:5121458888b5 155
raduschirila 0:5121458888b5 156
raduschirila 0:5121458888b5 157 break;
raduschirila 0:5121458888b5 158
raduschirila 0:5121458888b5 159 default:
raduschirila 0:5121458888b5 160
raduschirila 0:5121458888b5 161 break;
raduschirila 0:5121458888b5 162
raduschirila 0:5121458888b5 163 }
raduschirila 0:5121458888b5 164
raduschirila 0:5121458888b5 165 pc.printf("Done!\n");
raduschirila 0:5121458888b5 166
raduschirila 0:5121458888b5 167 memset(command,NULL, sizeof(command));
raduschirila 0:5121458888b5 168
raduschirila 0:5121458888b5 169
raduschirila 0:5121458888b5 170 }
raduschirila 0:5121458888b5 171
raduschirila 0:5121458888b5 172 else if(c=='c')//codename for color detection
raduschirila 0:5121458888b5 173
raduschirila 0:5121458888b5 174 {
raduschirila 0:5121458888b5 175 int col;
raduschirila 0:5121458888b5 176 col=detect_color();
raduschirila 0:5121458888b5 177
raduschirila 0:5121458888b5 178 pc.printf("%d\n",col);
raduschirila 0:5121458888b5 179
raduschirila 0:5121458888b5 180 }
raduschirila 0:5121458888b5 181
raduschirila 0:5121458888b5 182 else if(c=='l')//codename for line detection
raduschirila 0:5121458888b5 183
raduschirila 0:5121458888b5 184 {
raduschirila 0:5121458888b5 185 pc.printf("%f\n",get_position());//placeholder code
raduschirila 0:5121458888b5 186
raduschirila 0:5121458888b5 187 }
raduschirila 0:5121458888b5 188 else if(c=='s')
raduschirila 0:5121458888b5 189 {
raduschirila 0:5121458888b5 190 if((int)solenoid.read() == 1)
raduschirila 0:5121458888b5 191 {
raduschirila 0:5121458888b5 192 solenoid=0;
raduschirila 0:5121458888b5 193 pc.printf("Solenoid disengaged\n");
raduschirila 0:5121458888b5 194 }
raduschirila 0:5121458888b5 195 else if ((int)solenoid.read() ==0)
raduschirila 0:5121458888b5 196 {
raduschirila 0:5121458888b5 197 solenoid = 1;
raduschirila 0:5121458888b5 198 pc.printf("Solenoid engaged\n");
raduschirila 0:5121458888b5 199 }
raduschirila 0:5121458888b5 200
raduschirila 0:5121458888b5 201 }
raduschirila 0:5121458888b5 202
raduschirila 0:5121458888b5 203 else
raduschirila 0:5121458888b5 204 {
raduschirila 0:5121458888b5 205 pc.printf("Command Unknown.\n");
raduschirila 0:5121458888b5 206 }
raduschirila 0:5121458888b5 207 }
raduschirila 0:5121458888b5 208 }
raduschirila 0:5121458888b5 209
raduschirila 0:5121458888b5 210
raduschirila 0:5121458888b5 211 int main()
raduschirila 0:5121458888b5 212 {
raduschirila 0:5121458888b5 213 wait(2);
raduschirila 0:5121458888b5 214 if(pc.readable())
raduschirila 0:5121458888b5 215 {
raduschirila 0:5121458888b5 216 if((char)pc.getc()=='z');
raduschirila 0:5121458888b5 217 command_mode();
raduschirila 0:5121458888b5 218 }
raduschirila 0:5121458888b5 219 else
raduschirila 0:5121458888b5 220 {
raduschirila 0:5121458888b5 221
raduschirila 0:5121458888b5 222 wait(2.0);
raduschirila 0:5121458888b5 223
raduschirila 0:5121458888b5 224 float right;
raduschirila 0:5121458888b5 225 float left;
raduschirila 0:5121458888b5 226 float current_pos_of_line = 0.0;
raduschirila 0:5121458888b5 227 float previous_pos_of_line = 0.0;
raduschirila 0:5121458888b5 228 float previous_derivative=0;
raduschirila 0:5121458888b5 229 float derivative,proportional,DoD = 0;
raduschirila 0:5121458888b5 230 float power;
raduschirila 0:5121458888b5 231 float speed = MAX;
raduschirila 0:5121458888b5 232 solenoid=1;
raduschirila 0:5121458888b5 233 while (1) {
raduschirila 0:5121458888b5 234 detect_color();
raduschirila 0:5121458888b5 235 // Get the position of the line.
raduschirila 0:5121458888b5 236 current_pos_of_line = get_position();
raduschirila 0:5121458888b5 237 proportional = current_pos_of_line;
raduschirila 0:5121458888b5 238
raduschirila 0:5121458888b5 239 // Compute the derivative
raduschirila 0:5121458888b5 240 derivative = current_pos_of_line - previous_pos_of_line;
raduschirila 0:5121458888b5 241
raduschirila 0:5121458888b5 242 // Compute the DoD
raduschirila 0:5121458888b5 243 DoD = derivative - previous_derivative;
raduschirila 0:5121458888b5 244
raduschirila 0:5121458888b5 245 // Remember the last position.
raduschirila 0:5121458888b5 246 previous_pos_of_line = current_pos_of_line;
raduschirila 0:5121458888b5 247 previous_derivative = derivative;
raduschirila 0:5121458888b5 248 // Compute the power
raduschirila 0:5121458888b5 249 power = k*(proportional * (P_TERM) ) + (DoD*(DD_TERM)) + (derivative*(D_TERM)) ;
raduschirila 0:5121458888b5 250
raduschirila 0:5121458888b5 251
raduschirila 0:5121458888b5 252
raduschirila 0:5121458888b5 253
raduschirila 0:5121458888b5 254 // Compute new speeds
raduschirila 0:5121458888b5 255 right = speed+power;
raduschirila 0:5121458888b5 256 left = speed-power;
raduschirila 0:5121458888b5 257
raduschirila 0:5121458888b5 258 // set speed
raduschirila 0:5121458888b5 259 drive(left,right);
raduschirila 0:5121458888b5 260 }
raduschirila 0:5121458888b5 261 }
raduschirila 0:5121458888b5 262 }