Thomas Glatzel / Mbed 2 deprecated newlib

cross-cross and t-cross problem

Dependencies:   m3pi_ng mbed btbee

main.cpp@1:f4dd7bc26785, 2013-05-17 (annotated)

Committer:
jomkippur
Date:
Fri May 17 11:14:34 2013 +0000
Revision:
1:f4dd7bc26785
Parent:
0:1e2ff5ae5204 
whatever

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jomkippur 0:1e2ff5ae5204 1 #include "mbed.h"
jomkippur 0:1e2ff5ae5204 2 #include "m3pi_ng.h"
jomkippur 1:f4dd7bc26785 3 #include "btbee.h"
jomkippur 0:1e2ff5ae5204 4
jomkippur 0:1e2ff5ae5204 5 // Minimum and maximum motor speeds
jomkippur 1:f4dd7bc26785 6 #define MAX 0.2
jomkippur 0:1e2ff5ae5204 7 #define MIN 0
jomkippur 0:1e2ff5ae5204 8
jomkippur 0:1e2ff5ae5204 9 // PID terms
jomkippur 0:1e2ff5ae5204 10 #define P_TERM 1
jomkippur 0:1e2ff5ae5204 11 #define I_TERM 0
jomkippur 1:f4dd7bc26785 12 #define D_TERM 2 // with one call printf
jomkippur 0:1e2ff5ae5204 13
jomkippur 1:f4dd7bc26785 14 // Declaration
jomkippur 1:f4dd7bc26785 15 m3pi pi; //pi is of the classtyp m3pi
jomkippur 1:f4dd7bc26785 16 btbee bee; //bee is of the classtyp mtbee
jomkippur 0:1e2ff5ae5204 17
jomkippur 0:1e2ff5ae5204 18
jomkippur 1:f4dd7bc26785 19 // shows the battery voltage in first line for a little? time ,
jomkippur 1:f4dd7bc26785 20 // how long takes one for
jomkippur 1:f4dd7bc26785 21 void battery_voltage()
jomkippur 1:f4dd7bc26785 22 {
jomkippur 1:f4dd7bc26785 23 float voltage;
jomkippur 1:f4dd7bc26785 24 //
jomkippur 1:f4dd7bc26785 25 for(int i=0; i<100; i++) {
jomkippur 0:1e2ff5ae5204 26 voltage=pi.battery();
jomkippur 0:1e2ff5ae5204 27 pi.locate(0,0);
jomkippur 0:1e2ff5ae5204 28 pi.printf("%f",voltage);
jomkippur 0:1e2ff5ae5204 29 pi.locate(0,1);
jomkippur 0:1e2ff5ae5204 30 pi.printf("%i",i);
jomkippur 1:f4dd7bc26785 31 }
jomkippur 1:f4dd7bc26785 32 pi.cls();
jomkippur 1:f4dd7bc26785 33 }
jomkippur 1:f4dd7bc26785 34
jomkippur 1:f4dd7bc26785 35 // function calibration //////
jomkippur 1:f4dd7bc26785 36 void calib()
jomkippur 1:f4dd7bc26785 37 {
jomkippur 1:f4dd7bc26785 38 char complete=pi.sensor_auto_calibrate();
jomkippur 1:f4dd7bc26785 39 // sensor_auto_calibrate, wie geht das intern?Atmel
jomkippur 1:f4dd7bc26785 40 if(complete=='c')
jomkippur 0:1e2ff5ae5204 41 {
jomkippur 1:f4dd7bc26785 42 pi.locate(0,0);
jomkippur 1:f4dd7bc26785 43 pi.printf("calibrat");
jomkippur 1:f4dd7bc26785 44 pi.locate(0,1);
jomkippur 1:f4dd7bc26785 45 pi.printf("complete");
jomkippur 1:f4dd7bc26785 46 wait(2);
jomkippur 1:f4dd7bc26785 47 }else
jomkippur 1:f4dd7bc26785 48 {
jomkippur 1:f4dd7bc26785 49 pi.locate(0,0);
jomkippur 1:f4dd7bc26785 50 pi.printf("calibrat");
jomkippur 1:f4dd7bc26785 51 pi.locate(0,1);
jomkippur 1:f4dd7bc26785 52 pi.printf("failed");
jomkippur 1:f4dd7bc26785 53 wait(2);
jomkippur 0:1e2ff5ae5204 54 }
jomkippur 1:f4dd7bc26785 55 pi.cls();
jomkippur 1:f4dd7bc26785 56 }
jomkippur 1:f4dd7bc26785 57
jomkippur 1:f4dd7bc26785 58
jomkippur 1:f4dd7bc26785 59
jomkippur 1:f4dd7bc26785 60
jomkippur 1:f4dd7bc26785 61
jomkippur 1:f4dd7bc26785 62 ///////////////////////////////////////////////////////////////////////////////////
jomkippur 1:f4dd7bc26785 63 // function gives back 1 if outer sensors on black surface ////////////////////////
jomkippur 1:f4dd7bc26785 64 ///////////////////////////////////////////////////////////////////////////////////
jomkippur 0:1e2ff5ae5204 65 //parameter "black_or_white" is a pointer, argument has to be a address
jomkippur 0:1e2ff5ae5204 66
jomkippur 0:1e2ff5ae5204 67 int outer_sens()//int* outer_b_or_w)
jomkippur 1:f4dd7bc26785 68 {
jomkippur 0:1e2ff5ae5204 69 int outer_b_or_w;
jomkippur 1:f4dd7bc26785 70 // read raw sensor values
jomkippur 1:f4dd7bc26785 71 //
jomkippur 0:1e2ff5ae5204 72 int raw_sens_values[5];
jomkippur 0:1e2ff5ae5204 73 pi.raw_sensor(raw_sens_values);
jomkippur 1:f4dd7bc26785 74
jomkippur 1:f4dd7bc26785 75 // give the raw sensorvalue in 1. line
jomkippur 1:f4dd7bc26785 76 /*
jomkippur 0:1e2ff5ae5204 77 pi.locate(0,0);
jomkippur 1:f4dd7bc26785 78 ///////////////////////////////////////////////////////////////////////////////////
jomkippur 1:f4dd7bc26785 79 wait(0.2);//value meassurement with delay
jomkippur 1:f4dd7bc26785 80 (not good but good for representation of the values on scream
jomkippur 1:f4dd7bc26785 81 ///////////////////////////////////////////////////////////////////////////////////
jomkippur 1:f4dd7bc26785 82
jomkippur 0:1e2ff5ae5204 83 pi.cls(); // ohne clear display zeigt er Werte über 2000 an
jomkippur 1:f4dd7bc26785 84 pi.printf("%d",raw_sens_values[0]);
jomkippur 0:1e2ff5ae5204 85 // array entspricht pointer
jomkippur 0:1e2ff5ae5204 86 */
jomkippur 1:f4dd7bc26785 87
jomkippur 0:1e2ff5ae5204 88 if (raw_sens_values[0]> 1000 & raw_sens_values[4]> 1000)// solange
jomkippur 1:f4dd7bc26785 89 //auf saal:white = ca. 400; black = ca. 2000;
jomkippur 1:f4dd7bc26785 90 {
jomkippur 0:1e2ff5ae5204 91 //pi.locate(0,1);
jomkippur 0:1e2ff5ae5204 92 //pi.printf("%i",1);
jomkippur 1:f4dd7bc26785 93 outer_b_or_w=1; //1 is put to the address where the
jomkippur 1:f4dd7bc26785 94 } //black // pointer is pointing at
jomkippur 1:f4dd7bc26785 95 else {
jomkippur 1:f4dd7bc26785 96 // pi.locate(0,1);
jomkippur 1:f4dd7bc26785 97 // pi.printf("%i",0);
jomkippur 1:f4dd7bc26785 98 outer_b_or_w=0;
jomkippur 0:1e2ff5ae5204 99
jomkippur 1:f4dd7bc26785 100 } //white
jomkippur 1:f4dd7bc26785 101
jomkippur 1:f4dd7bc26785 102 // return by ???
jomkippur 1:f4dd7bc26785 103 // return by value is slow far structures and arrays !!!!!!!!!!!!!!!!!!????????
jomkippur 1:f4dd7bc26785 104 return outer_b_or_w;
jomkippur 1:f4dd7bc26785 105 }
jomkippur 1:f4dd7bc26785 106
jomkippur 1:f4dd7bc26785 107 // function gives back 1 if inner sensors on black surface ////////////////////////
jomkippur 1:f4dd7bc26785 108 ///////////////////////////////////////////////////////////////////////////////////
jomkippur 0:1e2ff5ae5204 109 //parameter "black_or_white" is a pointer, argument has to be a address
jomkippur 0:1e2ff5ae5204 110
jomkippur 0:1e2ff5ae5204 111 int inner_sens()
jomkippur 1:f4dd7bc26785 112 {
jomkippur 1:f4dd7bc26785 113 int inner_b_or_w;
jomkippur 0:1e2ff5ae5204 114 int raw_sens_values[5];
jomkippur 0:1e2ff5ae5204 115 pi.raw_sensor(raw_sens_values);
jomkippur 0:1e2ff5ae5204 116
jomkippur 0:1e2ff5ae5204 117
jomkippur 1:f4dd7bc26785 118 if (raw_sens_values[2]> 1000)// solange
jomkippur 1:f4dd7bc26785 119 //white = ca. 400; black = ca. 2000;
jomkippur 1:f4dd7bc26785 120 {
jomkippur 1:f4dd7bc26785 121 inner_b_or_w=1; //1 is put to the address where the
jomkippur 1:f4dd7bc26785 122 } //black // pointer is pointing at
jomkippur 1:f4dd7bc26785 123 else {
jomkippur 1:f4dd7bc26785 124 inner_b_or_w=0;
jomkippur 1:f4dd7bc26785 125
jomkippur 1:f4dd7bc26785 126 }
jomkippur 1:f4dd7bc26785 127 return inner_b_or_w;
jomkippur 1:f4dd7bc26785 128 }
jomkippur 1:f4dd7bc26785 129
jomkippur 1:f4dd7bc26785 130
jomkippur 1:f4dd7bc26785 131
jomkippur 1:f4dd7bc26785 132
jomkippur 1:f4dd7bc26785 133 ///////////////////////////////////////////////////////////////////////////////////
jomkippur 0:1e2ff5ae5204 134 // function line_follow ///////////////////////////////////////////////////////////
jomkippur 0:1e2ff5ae5204 135 ///////////////////////////////////////////////////////////////////////////////////
jomkippur 0:1e2ff5ae5204 136
jomkippur 1:f4dd7bc26785 137 void line_follow(float * integrator_ptr, float * prev_line_ptr)
jomkippur 1:f4dd7bc26785 138 // argument passed by address,
jomkippur 1:f4dd7bc26785 139 // integrator_ptr is a pointer that points to an address
jomkippur 1:f4dd7bc26785 140 // in main
jomkippur 1:f4dd7bc26785 141
jomkippur 0:1e2ff5ae5204 142 {
jomkippur 0:1e2ff5ae5204 143
jomkippur 0:1e2ff5ae5204 144
jomkippur 0:1e2ff5ae5204 145 // Defines on top
jomkippur 0:1e2ff5ae5204 146 pi.locate(0,1);
jomkippur 0:1e2ff5ae5204 147 pi.printf("Line PID");
jomkippur 1:f4dd7bc26785 148
jomkippur 0:1e2ff5ae5204 149 //wait(2.0);
jomkippur 1:f4dd7bc26785 150
jomkippur 0:1e2ff5ae5204 151
jomkippur 0:1e2ff5ae5204 152 float right;
jomkippur 0:1e2ff5ae5204 153 float left;
jomkippur 0:1e2ff5ae5204 154 float current_pos_of_line = 0.0;
jomkippur 1:f4dd7bc26785 155 //float previous_pos_of_line = 0.0;
jomkippur 0:1e2ff5ae5204 156 float derivative,proportional,integral = 0;
jomkippur 1:f4dd7bc26785 157 float diff_speed;
jomkippur 0:1e2ff5ae5204 158 float speed = MAX;
jomkippur 1:f4dd7bc26785 159
jomkippur 1:f4dd7bc26785 160 //integral = *integrator_ptr;
jomkippur 1:f4dd7bc26785 161 //previous_pos_of_line = *prev_line_ptr;
jomkippur 1:f4dd7bc26785 162
jomkippur 1:f4dd7bc26785 163
jomkippur 1:f4dd7bc26785 164 // Get the position of the line.
jomkippur 1:f4dd7bc26785 165 current_pos_of_line = pi.line_position();
jomkippur 1:f4dd7bc26785 166 proportional = current_pos_of_line;
jomkippur 1:f4dd7bc26785 167
jomkippur 1:f4dd7bc26785 168 // Compute the derivative
jomkippur 1:f4dd7bc26785 169 derivative = current_pos_of_line - *prev_line_ptr;//previous_pos_of_line;
jomkippur 1:f4dd7bc26785 170 // pointer points to the address where the
jomkippur 1:f4dd7bc26785 171 // value of the previous position x(t_{i-1}) is stored
jomkippur 1:f4dd7bc26785 172
jomkippur 1:f4dd7bc26785 173 // Compute the integral
jomkippur 1:f4dd7bc26785 174 integral += proportional;
jomkippur 1:f4dd7bc26785 175 *integrator_ptr = integral;
jomkippur 0:1e2ff5ae5204 176
jomkippur 0:1e2ff5ae5204 177
jomkippur 1:f4dd7bc26785 178 // Remember the last position.
jomkippur 1:f4dd7bc26785 179 //previous_pos_of_line = current_pos_of_line;
jomkippur 1:f4dd7bc26785 180 *prev_line_ptr = current_pos_of_line; // x(t_i) is put to the address
jomkippur 1:f4dd7bc26785 181 // where
jomkippur 1:f4dd7bc26785 182
jomkippur 1:f4dd7bc26785 183 // Compute the diff_speed
jomkippur 1:f4dd7bc26785 184
jomkippur 1:f4dd7bc26785 185 diff_speed = (proportional * (P_TERM) ) + (*integrator_ptr*(I_TERM)) + (derivative*(D_TERM)) ;
jomkippur 1:f4dd7bc26785 186 // Compute new speeds
jomkippur 1:f4dd7bc26785 187 right = speed+diff_speed;
jomkippur 1:f4dd7bc26785 188 left = speed-diff_speed;
jomkippur 0:1e2ff5ae5204 189
jomkippur 1:f4dd7bc26785 190 // limit checks ////////////////////////
jomkippur 1:f4dd7bc26785 191 if (right < MIN)
jomkippur 1:f4dd7bc26785 192 right = MIN;
jomkippur 1:f4dd7bc26785 193 else if (right > MAX)
jomkippur 1:f4dd7bc26785 194 right = MAX;
jomkippur 0:1e2ff5ae5204 195
jomkippur 1:f4dd7bc26785 196 if (left < MIN)
jomkippur 1:f4dd7bc26785 197 left = MIN;
jomkippur 1:f4dd7bc26785 198 else if (left > MAX)
jomkippur 1:f4dd7bc26785 199 left = MAX;
jomkippur 1:f4dd7bc26785 200
jomkippur 1:f4dd7bc26785 201 // set speed
jomkippur 1:f4dd7bc26785 202 pi.left_motor(left);
jomkippur 1:f4dd7bc26785 203 pi.right_motor(right);
jomkippur 1:f4dd7bc26785 204
jomkippur 1:f4dd7bc26785 205 }
jomkippur 1:f4dd7bc26785 206
jomkippur 1:f4dd7bc26785 207 ///////////////////////////////////////////////////////////////////////////////////
jomkippur 1:f4dd7bc26785 208 // function turn right ////////////////////////////////////////////////////////////
jomkippur 0:1e2ff5ae5204 209 ///////////////////////////////////////////////////////////////////////////////////
jomkippur 0:1e2ff5ae5204 210 void turn_right()
jomkippur 1:f4dd7bc26785 211 {
jomkippur 0:1e2ff5ae5204 212 float turn_speed = MAX;
jomkippur 1:f4dd7bc26785 213 pi.left_motor(-turn_speed);// look on the wheel, pos speed => wheel turns clockwise
jomkippur 1:f4dd7bc26785 214 pi.right_motor(turn_speed);
jomkippur 1:f4dd7bc26785 215 wait (0.9);
jomkippur 1:f4dd7bc26785 216 }
jomkippur 1:f4dd7bc26785 217
jomkippur 1:f4dd7bc26785 218
jomkippur 1:f4dd7bc26785 219
jomkippur 1:f4dd7bc26785 220 ///////////////////////////////////////////////////////////////////////////////////
jomkippur 1:f4dd7bc26785 221 // main
jomkippur 0:1e2ff5ae5204 222 ///////////////////////////////////////////////////////////////////////////////////
jomkippur 0:1e2ff5ae5204 223
jomkippur 0:1e2ff5ae5204 224
jomkippur 1:f4dd7bc26785 225 main()
jomkippur 0:1e2ff5ae5204 226 {
jomkippur 1:f4dd7bc26785 227 // declaration //////
jomkippur 1:f4dd7bc26785 228
jomkippur 1:f4dd7bc26785 229 float cross_time;
jomkippur 1:f4dd7bc26785 230 float cross_speed;
jomkippur 1:f4dd7bc26785 231 float integrator;
jomkippur 1:f4dd7bc26785 232 float prev_line_pos;
jomkippur 1:f4dd7bc26785 233
jomkippur 1:f4dd7bc26785 234 // initialation ////
jomkippur 0:1e2ff5ae5204 235
jomkippur 1:f4dd7bc26785 236 //how long does it take to move over the cross?
jomkippur 1:f4dd7bc26785 237 // 9.5/2cm=4.25cm // half diameter of the m3pi
jomkippur 1:f4dd7bc26785 238 // 0 < speed < 1
jomkippur 1:f4dd7bc26785 239 // measure: speed1=0.1 s1=42cm t1=5.5s voltage1=4.4 V
jomkippur 1:f4dd7bc26785 240 // measure: speed1=0.2 s1=42cm t1=2.1s
jomkippur 1:f4dd7bc26785 241
jomkippur 1:f4dd7bc26785 242 // real speed1=42/5.5 cm/s=7.6 cm/s
jomkippur 1:f4dd7bc26785 243 // dt1=4.25/7.6s=0.6s
jomkippur 1:f4dd7bc26785 244 // real speed2=42/2.1 cm/s=20 cm/s
jomkippur 1:f4dd7bc26785 245 // dt2=4.24/20s=0.212s
jomkippur 0:1e2ff5ae5204 246
jomkippur 1:f4dd7bc26785 247 cross_time=0.22;
jomkippur 1:f4dd7bc26785 248
jomkippur 1:f4dd7bc26785 249 integrator=0.0;
jomkippur 1:f4dd7bc26785 250 prev_line_pos=0.0;
jomkippur 1:f4dd7bc26785 251 cross_speed = MAX*0.6;// MAX=0.2 60%
jomkippur 1:f4dd7bc26785 252
jomkippur 1:f4dd7bc26785 253
jomkippur 1:f4dd7bc26785 254 pi.cls();
jomkippur 0:1e2ff5ae5204 255 battery_voltage();
jomkippur 0:1e2ff5ae5204 256 calib();
jomkippur 1:f4dd7bc26785 257
jomkippur 1:f4dd7bc26785 258
jomkippur 1:f4dd7bc26785 259
jomkippur 1:f4dd7bc26785 260 while(true)
jomkippur 1:f4dd7bc26785 261 {
jomkippur 1:f4dd7bc26785 262
jomkippur 1:f4dd7bc26785 263 if(outer_sens()==0 && inner_sens()==1)//outer white, inner black
jomkippur 1:f4dd7bc26785 264 {
jomkippur 1:f4dd7bc26785 265 //pi.locate(0,0);
jomkippur 1:f4dd7bc26785 266 //pi.printf("while %i",outer_sens());
jomkippur 1:f4dd7bc26785 267 //pi.cls();
jomkippur 1:f4dd7bc26785 268 line_follow(&integrator, &prev_line_pos);// zurücksetzen von integrator und pos x(t_i)=0
jomkippur 1:f4dd7bc26785 269 }
jomkippur 1:f4dd7bc26785 270 else if(outer_sens()==1 && inner_sens()==1 ) // outer black, inner black
jomkippur 1:f4dd7bc26785 271 {
jomkippur 1:f4dd7bc26785 272 pi.cls();
jomkippur 1:f4dd7bc26785 273 pi.stop();
jomkippur 1:f4dd7bc26785 274 pi.locate(0,0);
jomkippur 1:f4dd7bc26785 275 pi.printf("cross");
jomkippur 1:f4dd7bc26785 276 pi.locate(0,1);
jomkippur 1:f4dd7bc26785 277 pi.printf("stop");
jomkippur 1:f4dd7bc26785 278 wait(0.2);
jomkippur 1:f4dd7bc26785 279 // move just little further
jomkippur 1:f4dd7bc26785 280 pi.left_motor(cross_speed);
jomkippur 1:f4dd7bc26785 281 pi.right_motor(cross_speed);
jomkippur 1:f4dd7bc26785 282 wait(cross_time);
jomkippur 1:f4dd7bc26785 283
jomkippur 1:f4dd7bc26785 284
jomkippur 0:1e2ff5ae5204 285 /////////////////////////////////////////////////////////////////////////
jomkippur 0:1e2ff5ae5204 286 // t-cross?
jomkippur 0:1e2ff5ae5204 287 //////////////////////////////////////////////////////////////////////////
jomkippur 1:f4dd7bc26785 288 if(outer_sens()==0 && inner_sens()==0 )// outer white, inner white
jomkippur 1:f4dd7bc26785 289
jomkippur 0:1e2ff5ae5204 290 {
jomkippur 1:f4dd7bc26785 291 pi.stop();
jomkippur 1:f4dd7bc26785 292 pi.cls();
jomkippur 1:f4dd7bc26785 293 pi.locate(0,0);
jomkippur 1:f4dd7bc26785 294 pi.printf("T end");
jomkippur 1:f4dd7bc26785 295 } else
jomkippur 1:f4dd7bc26785 296 /////////////////////////////////////////////////////////////////////////
jomkippur 1:f4dd7bc26785 297 // cross-cross
jomkippur 1:f4dd7bc26785 298 if(outer_sens()==0 && inner_sens()==1 ) { // outer white, inner black
jomkippur 1:f4dd7bc26785 299 //turn_right();
jomkippur 1:f4dd7bc26785 300
jomkippur 1:f4dd7bc26785 301 }//second else
jomkippur 1:f4dd7bc26785 302 }//first else
jomkippur 1:f4dd7bc26785 303 }//while true
jomkippur 1:f4dd7bc26785 304
jomkippur 1:f4dd7bc26785 305 }