Thomas Glatzel / Mbed 2 deprecated newlib

cross-cross and t-cross problem

Dependencies:   m3pi_ng mbed btbee

main.cpp@0:1e2ff5ae5204, 2013-05-16 (annotated)

Committer:
jomkippur
Date:
Thu May 16 10:32:00 2013 +0000
Revision:
0:1e2ff5ae5204
Child:
1:f4dd7bc26785
cross-cross and t-cross problem

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 0:1e2ff5ae5204 3
jomkippur 0:1e2ff5ae5204 4
jomkippur 0:1e2ff5ae5204 5 // Minimum and maximum motor speeds
jomkippur 0:1e2ff5ae5204 6 #define MAX 0.1
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 0:1e2ff5ae5204 12 #define D_TERM 20
jomkippur 0:1e2ff5ae5204 13
jomkippur 0:1e2ff5ae5204 14
jomkippur 0:1e2ff5ae5204 15
jomkippur 0:1e2ff5ae5204 16
jomkippur 0:1e2ff5ae5204 17
jomkippur 0:1e2ff5ae5204 18 // Declaration pi is of the classtyp m3pi
jomkippur 0:1e2ff5ae5204 19 m3pi pi;
jomkippur 0:1e2ff5ae5204 20
jomkippur 0:1e2ff5ae5204 21 // shows the battery voltage in first line for a little? time , how long takes one for
jomkippur 0:1e2ff5ae5204 22 void battery_voltage()
jomkippur 0:1e2ff5ae5204 23 {
jomkippur 0:1e2ff5ae5204 24 float voltage;
jomkippur 0:1e2ff5ae5204 25 //
jomkippur 0:1e2ff5ae5204 26 for(int i=0;i<100;i++)
jomkippur 0:1e2ff5ae5204 27 {
jomkippur 0:1e2ff5ae5204 28 voltage=pi.battery();
jomkippur 0:1e2ff5ae5204 29 pi.locate(0,0);
jomkippur 0:1e2ff5ae5204 30 pi.printf("%f",voltage);
jomkippur 0:1e2ff5ae5204 31 pi.locate(0,1);
jomkippur 0:1e2ff5ae5204 32 pi.printf("%i",i);
jomkippur 0:1e2ff5ae5204 33 }
jomkippur 0:1e2ff5ae5204 34 pi.cls();
jomkippur 0:1e2ff5ae5204 35 }
jomkippur 0:1e2ff5ae5204 36
jomkippur 0:1e2ff5ae5204 37 // calibration with c for complete on screen
jomkippur 0:1e2ff5ae5204 38 void calib()
jomkippur 0:1e2ff5ae5204 39 {
jomkippur 0:1e2ff5ae5204 40 char complete=pi.sensor_auto_calibrate(); //wozu denn?
jomkippur 0:1e2ff5ae5204 41 pi.locate(0,0);
jomkippur 0:1e2ff5ae5204 42 pi.printf("%c",complete);
jomkippur 0:1e2ff5ae5204 43 wait(2);
jomkippur 0:1e2ff5ae5204 44 pi.cls();
jomkippur 0:1e2ff5ae5204 45 }
jomkippur 0:1e2ff5ae5204 46
jomkippur 0:1e2ff5ae5204 47
jomkippur 0:1e2ff5ae5204 48
jomkippur 0:1e2ff5ae5204 49
jomkippur 0:1e2ff5ae5204 50
jomkippur 0:1e2ff5ae5204 51
jomkippur 0:1e2ff5ae5204 52 // function gives back 1 if outer sensors on black surface //////////////////////////////////////////
jomkippur 0:1e2ff5ae5204 53 /////////////////////////////////////////////////////////////////////////////////////////////////////
jomkippur 0:1e2ff5ae5204 54 //parameter "black_or_white" is a pointer, argument has to be a address
jomkippur 0:1e2ff5ae5204 55
jomkippur 0:1e2ff5ae5204 56 int outer_sens()//int* outer_b_or_w)
jomkippur 0:1e2ff5ae5204 57 {
jomkippur 0:1e2ff5ae5204 58 int outer_b_or_w;
jomkippur 0:1e2ff5ae5204 59 // read raw sensor values
jomkippur 0:1e2ff5ae5204 60 //
jomkippur 0:1e2ff5ae5204 61 int raw_sens_values[5];
jomkippur 0:1e2ff5ae5204 62 pi.raw_sensor(raw_sens_values);
jomkippur 0:1e2ff5ae5204 63
jomkippur 0:1e2ff5ae5204 64 // give the raw sensorvalue in 1. line
jomkippur 0:1e2ff5ae5204 65 /*
jomkippur 0:1e2ff5ae5204 66 pi.locate(0,0);
jomkippur 0:1e2ff5ae5204 67 ///////////////////////////////////////
jomkippur 0:1e2ff5ae5204 68 wait(0.2);//value meassurement with delay (not good but good for representation of the values on scream
jomkippur 0:1e2ff5ae5204 69 ///////////////////////////////////////
jomkippur 0:1e2ff5ae5204 70
jomkippur 0:1e2ff5ae5204 71 pi.cls(); // ohne clear display zeigt er Werte über 2000 an
jomkippur 0:1e2ff5ae5204 72 pi.printf("%d",raw_sens_values[0]);
jomkippur 0:1e2ff5ae5204 73 // array entspricht pointer
jomkippur 0:1e2ff5ae5204 74 */
jomkippur 0:1e2ff5ae5204 75
jomkippur 0:1e2ff5ae5204 76 if (raw_sens_values[0]> 1000 & raw_sens_values[4]> 1000)// solange
jomkippur 0:1e2ff5ae5204 77 //auf saal:white = ca. 400; black = ca. 2000;
jomkippur 0:1e2ff5ae5204 78 {
jomkippur 0:1e2ff5ae5204 79 //pi.locate(0,1);
jomkippur 0:1e2ff5ae5204 80 //pi.printf("%i",1);
jomkippur 0:1e2ff5ae5204 81 outer_b_or_w=1; //1 is put to the address where the
jomkippur 0:1e2ff5ae5204 82 } //black // pointer is pointing at
jomkippur 0:1e2ff5ae5204 83 else
jomkippur 0:1e2ff5ae5204 84 {
jomkippur 0:1e2ff5ae5204 85 // pi.locate(0,1);
jomkippur 0:1e2ff5ae5204 86 // pi.printf("%i",0);
jomkippur 0:1e2ff5ae5204 87 outer_b_or_w=0;
jomkippur 0:1e2ff5ae5204 88
jomkippur 0:1e2ff5ae5204 89 } //white
jomkippur 0:1e2ff5ae5204 90
jomkippur 0:1e2ff5ae5204 91 // return by ???
jomkippur 0:1e2ff5ae5204 92 // return by value is slow far structures and arrays !!!!!!!!!!!!!!!!!!???????????????????
jomkippur 0:1e2ff5ae5204 93 return outer_b_or_w;
jomkippur 0:1e2ff5ae5204 94 }
jomkippur 0:1e2ff5ae5204 95
jomkippur 0:1e2ff5ae5204 96 // function gives back 1 if inner sensors on black surface //////////////////////////////////////////
jomkippur 0:1e2ff5ae5204 97 /////////////////////////////////////////////////////////////////////////////////////////////////////
jomkippur 0:1e2ff5ae5204 98 //parameter "black_or_white" is a pointer, argument has to be a address
jomkippur 0:1e2ff5ae5204 99
jomkippur 0:1e2ff5ae5204 100 int inner_sens()
jomkippur 0:1e2ff5ae5204 101 {
jomkippur 0:1e2ff5ae5204 102 int inner_b_or_w;
jomkippur 0:1e2ff5ae5204 103 int raw_sens_values[5];
jomkippur 0:1e2ff5ae5204 104 pi.raw_sensor(raw_sens_values);
jomkippur 0:1e2ff5ae5204 105
jomkippur 0:1e2ff5ae5204 106
jomkippur 0:1e2ff5ae5204 107 if (raw_sens_values[2]> 1000)// solange
jomkippur 0:1e2ff5ae5204 108 //white = ca. 400; black = ca. 2000;
jomkippur 0:1e2ff5ae5204 109 {
jomkippur 0:1e2ff5ae5204 110 inner_b_or_w=1; //1 is put to the address where the
jomkippur 0:1e2ff5ae5204 111 } //black // pointer is pointing at
jomkippur 0:1e2ff5ae5204 112 else
jomkippur 0:1e2ff5ae5204 113 {
jomkippur 0:1e2ff5ae5204 114 inner_b_or_w=0;
jomkippur 0:1e2ff5ae5204 115
jomkippur 0:1e2ff5ae5204 116 }
jomkippur 0:1e2ff5ae5204 117 return inner_b_or_w;
jomkippur 0:1e2ff5ae5204 118 }
jomkippur 0:1e2ff5ae5204 119
jomkippur 0:1e2ff5ae5204 120
jomkippur 0:1e2ff5ae5204 121
jomkippur 0:1e2ff5ae5204 122
jomkippur 0:1e2ff5ae5204 123 ///////////////////////////////////////////////////////////////////////////////////
jomkippur 0:1e2ff5ae5204 124 // function line_follow ///////////////////////////////////////////////////////////
jomkippur 0:1e2ff5ae5204 125 ///////////////////////////////////////////////////////////////////////////////////
jomkippur 0:1e2ff5ae5204 126
jomkippur 0:1e2ff5ae5204 127 //void line_follow(const int* ON)// runs only if ON points to a integer 1
jomkippur 0:1e2ff5ae5204 128 void line_follow()
jomkippur 0:1e2ff5ae5204 129 // argument passed by address, ON is a pointer that points to an address
jomkippur 0:1e2ff5ae5204 130 {
jomkippur 0:1e2ff5ae5204 131
jomkippur 0:1e2ff5ae5204 132
jomkippur 0:1e2ff5ae5204 133 // Defines on top
jomkippur 0:1e2ff5ae5204 134
jomkippur 0:1e2ff5ae5204 135
jomkippur 0:1e2ff5ae5204 136
jomkippur 0:1e2ff5ae5204 137
jomkippur 0:1e2ff5ae5204 138 pi.locate(0,1);
jomkippur 0:1e2ff5ae5204 139 pi.printf("Line PID");
jomkippur 0:1e2ff5ae5204 140
jomkippur 0:1e2ff5ae5204 141 //wait(2.0);
jomkippur 0:1e2ff5ae5204 142
jomkippur 0:1e2ff5ae5204 143
jomkippur 0:1e2ff5ae5204 144 float right;
jomkippur 0:1e2ff5ae5204 145 float left;
jomkippur 0:1e2ff5ae5204 146 float current_pos_of_line = 0.0;
jomkippur 0:1e2ff5ae5204 147 float previous_pos_of_line = 0.0;
jomkippur 0:1e2ff5ae5204 148 float derivative,proportional,integral = 0;
jomkippur 0:1e2ff5ae5204 149 float power;
jomkippur 0:1e2ff5ae5204 150 float speed = MAX;
jomkippur 0:1e2ff5ae5204 151
jomkippur 0:1e2ff5ae5204 152
jomkippur 0:1e2ff5ae5204 153 // Get the position of the line.
jomkippur 0:1e2ff5ae5204 154 current_pos_of_line = pi.line_position();
jomkippur 0:1e2ff5ae5204 155 proportional = current_pos_of_line;
jomkippur 0:1e2ff5ae5204 156
jomkippur 0:1e2ff5ae5204 157 // Compute the derivative
jomkippur 0:1e2ff5ae5204 158 derivative = current_pos_of_line - previous_pos_of_line;
jomkippur 0:1e2ff5ae5204 159
jomkippur 0:1e2ff5ae5204 160 // Compute the integral
jomkippur 0:1e2ff5ae5204 161 integral += proportional;
jomkippur 0:1e2ff5ae5204 162
jomkippur 0:1e2ff5ae5204 163 // Remember the last position.
jomkippur 0:1e2ff5ae5204 164 previous_pos_of_line = current_pos_of_line;
jomkippur 0:1e2ff5ae5204 165
jomkippur 0:1e2ff5ae5204 166 // Compute the power
jomkippur 0:1e2ff5ae5204 167 power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
jomkippur 0:1e2ff5ae5204 168
jomkippur 0:1e2ff5ae5204 169 // Compute new speeds
jomkippur 0:1e2ff5ae5204 170 right = speed+power;
jomkippur 0:1e2ff5ae5204 171 left = speed-power;
jomkippur 0:1e2ff5ae5204 172
jomkippur 0:1e2ff5ae5204 173 // limit checks
jomkippur 0:1e2ff5ae5204 174 if (right < MIN)
jomkippur 0:1e2ff5ae5204 175 right = MIN;
jomkippur 0:1e2ff5ae5204 176 else if (right > MAX)
jomkippur 0:1e2ff5ae5204 177 right = MAX;
jomkippur 0:1e2ff5ae5204 178
jomkippur 0:1e2ff5ae5204 179 if (left < MIN)
jomkippur 0:1e2ff5ae5204 180 left = MIN;
jomkippur 0:1e2ff5ae5204 181 else if (left > MAX)
jomkippur 0:1e2ff5ae5204 182 left = MAX;
jomkippur 0:1e2ff5ae5204 183
jomkippur 0:1e2ff5ae5204 184 // set speed
jomkippur 0:1e2ff5ae5204 185 pi.left_motor(left);
jomkippur 0:1e2ff5ae5204 186 pi.right_motor(right);
jomkippur 0:1e2ff5ae5204 187
jomkippur 0:1e2ff5ae5204 188 }
jomkippur 0:1e2ff5ae5204 189
jomkippur 0:1e2ff5ae5204 190 ///////////////////////////////////////////////////////////////////////////////////
jomkippur 0:1e2ff5ae5204 191 // function turn right ///////////////////////////////////////////////////////////
jomkippur 0:1e2ff5ae5204 192 ///////////////////////////////////////////////////////////////////////////////////
jomkippur 0:1e2ff5ae5204 193 void turn_right()
jomkippur 0:1e2ff5ae5204 194 {
jomkippur 0:1e2ff5ae5204 195 float turn_speed = MAX;
jomkippur 0:1e2ff5ae5204 196 pi.left_motor(-turn_speed);// look on the wheel, pos speed => wheel turns clockwise
jomkippur 0:1e2ff5ae5204 197 pi.right_motor(turn_speed);
jomkippur 0:1e2ff5ae5204 198 wait (0.9);
jomkippur 0:1e2ff5ae5204 199 }
jomkippur 0:1e2ff5ae5204 200 ///////////////////////////////////////////////////////////////////////////////////
jomkippur 0:1e2ff5ae5204 201 // main
jomkippur 0:1e2ff5ae5204 202 ///////////////////////////////////////////////////////////////////////////////////
jomkippur 0:1e2ff5ae5204 203
jomkippur 0:1e2ff5ae5204 204
jomkippur 0:1e2ff5ae5204 205
jomkippur 0:1e2ff5ae5204 206 main()
jomkippur 0:1e2ff5ae5204 207 {
jomkippur 0:1e2ff5ae5204 208
jomkippur 0:1e2ff5ae5204 209 float cross_speed = MAX*0.9;
jomkippur 0:1e2ff5ae5204 210
jomkippur 0:1e2ff5ae5204 211
jomkippur 0:1e2ff5ae5204 212 battery_voltage();
jomkippur 0:1e2ff5ae5204 213 calib();
jomkippur 0:1e2ff5ae5204 214
jomkippur 0:1e2ff5ae5204 215 while(true)
jomkippur 0:1e2ff5ae5204 216 {
jomkippur 0:1e2ff5ae5204 217
jomkippur 0:1e2ff5ae5204 218 if(outer_sens()==0 && inner_sens()==1)//outer white, inner black
jomkippur 0:1e2ff5ae5204 219 {
jomkippur 0:1e2ff5ae5204 220 pi.locate(0,0);
jomkippur 0:1e2ff5ae5204 221 pi.printf("while %i",outer_sens());
jomkippur 0:1e2ff5ae5204 222 pi.cls();
jomkippur 0:1e2ff5ae5204 223 line_follow();
jomkippur 0:1e2ff5ae5204 224 }
jomkippur 0:1e2ff5ae5204 225 else if(outer_sens()==1 && inner_sens()==1 )// outer black, inner black
jomkippur 0:1e2ff5ae5204 226 {
jomkippur 0:1e2ff5ae5204 227 pi.cls();
jomkippur 0:1e2ff5ae5204 228 pi.stop();
jomkippur 0:1e2ff5ae5204 229 pi.locate(0,0);
jomkippur 0:1e2ff5ae5204 230 pi.printf("stop");
jomkippur 0:1e2ff5ae5204 231 wait(0.2);
jomkippur 0:1e2ff5ae5204 232 // move just little further
jomkippur 0:1e2ff5ae5204 233 pi.left_motor(cross_speed);
jomkippur 0:1e2ff5ae5204 234 pi.right_motor(cross_speed);
jomkippur 0:1e2ff5ae5204 235 wait(0.5);
jomkippur 0:1e2ff5ae5204 236 //die zeit die erbraucht für den halben durch messer
jomkippur 0:1e2ff5ae5204 237 // 9.5/2cm=4.25cm
jomkippur 0:1e2ff5ae5204 238 // 0 < speed < 1
jomkippur 0:1e2ff5ae5204 239 // measure: speed=0.1 s=42cm t=5.5s voltage=4.4 V
jomkippur 0:1e2ff5ae5204 240 // real speed=42/5.5 cm/s=7.6 cm/s
jomkippur 0:1e2ff5ae5204 241 // dt=4.25/7.6=0.6
jomkippur 0:1e2ff5ae5204 242
jomkippur 0:1e2ff5ae5204 243 /////////////////////////////////////////////////////////////////////////
jomkippur 0:1e2ff5ae5204 244 // t-cross?
jomkippur 0:1e2ff5ae5204 245 if(outer_sens()==0 && inner_sens()==0 )// outer white, inner white
jomkippur 0:1e2ff5ae5204 246 //////////////////////////////////////////////////////////////////////////
jomkippur 0:1e2ff5ae5204 247 {
jomkippur 0:1e2ff5ae5204 248 pi.cls();
jomkippur 0:1e2ff5ae5204 249 pi.stop();
jomkippur 0:1e2ff5ae5204 250 pi.locate(0,0);
jomkippur 0:1e2ff5ae5204 251 pi.printf("T end");
jomkippur 0:1e2ff5ae5204 252 }
jomkippur 0:1e2ff5ae5204 253 else
jomkippur 0:1e2ff5ae5204 254 /////////////////////////////////////////////////////////////////////////
jomkippur 0:1e2ff5ae5204 255 // cross-cross
jomkippur 0:1e2ff5ae5204 256 if(outer_sens()==0 && inner_sens()==1 )// outer white, inner black
jomkippur 0:1e2ff5ae5204 257 {//turn_right();
jomkippur 0:1e2ff5ae5204 258 }
jomkippur 0:1e2ff5ae5204 259 }
jomkippur 0:1e2ff5ae5204 260 }
jomkippur 0:1e2ff5ae5204 261 }