fatboyslim / Mbed 2 deprecated buttontest

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:   FRDM-TFC mbed

main.cpp@29:91bda3a63f17, 2015-03-03 (annotated)

Committer:
mperella
Date:
Tue Mar 03 19:53:12 2015 +0000
Revision:
29:91bda3a63f17
Parent:
28:06cefc8dd15e
Child:
30:dfa9616b1ea4
Child:
33:e14ef9206a63
fixed compiler errors

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bbbobbbieo 3:c7caa058fc50 1 //#include "mbed.h"
bbbobbbieo 1:21d40d90b2f0 2 #include "TFC.h"
bbbobbbieo 0:d57117b2188d 3
bbergandy 25:1d0f586aaf0c 4 #define AGGRESSIVE .55
bbergandy 25:1d0f586aaf0c 5 #define MODERATE .48
bbergandy 25:1d0f586aaf0c 6 #define CONSERVATIVE .35
bbergandy 25:1d0f586aaf0c 7 #define STOP 0
bbergandy 25:1d0f586aaf0c 8 #define BLACK_THRESHOLD 64
bbergandy 25:1d0f586aaf0c 9
bbbobbbieo 0:d57117b2188d 10 DigitalOut myled(LED1);
bbbobbbieo 0:d57117b2188d 11
bbbobbbieo 14:f43b386b8b5d 12 int main()
bbbobbbieo 1:21d40d90b2f0 13 {
bbbobbbieo 7:455e7dd338ee 14 //run this before anything
bbbobbbieo 2:d8a51492b646 15 TFC_Init();
bbbobbbieo 14:f43b386b8b5d 16
bbbobbbieo 7:455e7dd338ee 17 //variables
bbbobbbieo 4:7584ff0426f1 18 float current_servo_position = 0;
bbbobbbieo 6:44d1079f076c 19 float current_left_motor_speed = 0;
bbbobbbieo 6:44d1079f076c 20 float current_right_motor_speed = 0;
mperella 17:c643b6b4a96f 21
mperella 17:c643b6b4a96f 22 float dt = 0.00001;
mperella 17:c643b6b4a96f 23 float integral = 0;
mperella 17:c643b6b4a96f 24 float derivative = 0;
mperella 17:c643b6b4a96f 25 float output = 0;
mperella 17:c643b6b4a96f 26
mperella 17:c643b6b4a96f 27 // gains on prop, int, der
mperella 17:c643b6b4a96f 28 // subject to change, need to fine tune
mperella 17:c643b6b4a96f 29 float kp = 1.8960;
mperella 17:c643b6b4a96f 30 float ki = 0.6170;
mperella 17:c643b6b4a96f 31 float kd = 1.5590;
mperella 17:c643b6b4a96f 32
bbbobbbieo 6:44d1079f076c 33 bool rear_motor_enable_flag = true;
bbbobbbieo 7:455e7dd338ee 34 bool linescan_ping_pong = false;
bbbobbbieo 9:2b028ee421ad 35 bool linescan_enable = true;
bbbobbbieo 14:f43b386b8b5d 36
bbbobbbieo 11:d65d6d7fc85e 37 int black_values_list[128];
bbbobbbieo 11:d65d6d7fc85e 38 int black_value_count = 0;
bbbobbbieo 11:d65d6d7fc85e 39 int black_center_value = 0;
bbbobbbieo 11:d65d6d7fc85e 40 int sum_black = 0;
bbbobbbieo 14:f43b386b8b5d 41 int violence_level = 0;
bbbobbbieo 15:830209e846d5 42
mperella 26:3f7ddc7d5bdf 43 int accelList[3];
mperella 26:3f7ddc7d5bdf 44 int lastAccessed = 0;
mperella 26:3f7ddc7d5bdf 45
mperella 28:06cefc8dd15e 46 int centers_List[50];
mperella 26:3f7ddc7d5bdf 47
mperella 28:06cefc8dd15e 48 int center_now = 63;
mperella 28:06cefc8dd15e 49 int center_past_1 = 63;
mperella 28:06cefc8dd15e 50 int center_past_2 = 63;
mperella 28:06cefc8dd15e 51 int center_past_3 = 63;
mperella 28:06cefc8dd15e 52 int center_past_4 = 63;
bbbobbbieo 20:c728b8ffad97 53 //int best_guess_center = 64;
bbbobbbieo 20:c728b8ffad97 54
mperella 28:06cefc8dd15e 55 int set_point = 63;
bbbobbbieo 19:85eb7991e2ab 56 int previous_error = 0;
mperella 17:c643b6b4a96f 57 int error = 0;
mperella 17:c643b6b4a96f 58
mperella 28:06cefc8dd15e 59 for(int i = 0; i < 50; i++)
mperella 28:06cefc8dd15e 60 centers_List[i] = 63;
mperella 17:c643b6b4a96f 61
bbbobbbieo 9:2b028ee421ad 62 //uint16_t MyImage0Buffer[2][128];
bbbobbbieo 9:2b028ee421ad 63 //uint16_t MyImage1Buffer[2][128];
bbbobbbieo 14:f43b386b8b5d 64
bbbobbbieo 7:455e7dd338ee 65 // major loop
bbbobbbieo 14:f43b386b8b5d 66 while(1) {
bbbobbbieo 14:f43b386b8b5d 67
bbbobbbieo 15:830209e846d5 68 // manual servo control, unused
bbbobbbieo 14:f43b386b8b5d 69 if (TFC_ReadPushButton(0) != 0 ) {
bbbobbbieo 14:f43b386b8b5d 70 current_servo_position = current_servo_position-.005;
bbbobbbieo 14:f43b386b8b5d 71 if(current_servo_position <= -0.4)
bbbobbbieo 14:f43b386b8b5d 72 current_servo_position = -0.4;
bbbobbbieo 14:f43b386b8b5d 73 TFC_SetServo(0, current_servo_position);
bbbobbbieo 14:f43b386b8b5d 74 }// end check button0
bbbobbbieo 14:f43b386b8b5d 75
bbbobbbieo 15:830209e846d5 76 else {}
mperella 28:06cefc8dd15e 77
bbbobbbieo 14:f43b386b8b5d 78
bbbobbbieo 15:830209e846d5 79 // manual servo control, unused
bbbobbbieo 14:f43b386b8b5d 80 if (TFC_ReadPushButton(1) != 0 ) {
bbbobbbieo 14:f43b386b8b5d 81 current_servo_position = current_servo_position+.005;
bbbobbbieo 14:f43b386b8b5d 82 if(current_servo_position >= 0.4)
bbbobbbieo 14:f43b386b8b5d 83 current_servo_position = 0.4;
bbbobbbieo 14:f43b386b8b5d 84 TFC_SetServo(0, current_servo_position);
bbbobbbieo 14:f43b386b8b5d 85 }// end check button1
bbbobbbieo 14:f43b386b8b5d 86
bbbobbbieo 15:830209e846d5 87 else {}
bbbobbbieo 14:f43b386b8b5d 88
bbbobbbieo 15:830209e846d5 89 // initial motor stuff
bbbobbbieo 14:f43b386b8b5d 90 if(rear_motor_enable_flag) {
bbbobbbieo 14:f43b386b8b5d 91 TFC_HBRIDGE_ENABLE;
bbbobbbieo 14:f43b386b8b5d 92
bbbobbbieo 15:830209e846d5 93 //current_left_motor_speed = (TFC_ReadPot(0));
bbbobbbieo 15:830209e846d5 94 //current_right_motor_speed = (TFC_ReadPot(1));
bbbobbbieo 14:f43b386b8b5d 95
bbbobbbieo 15:830209e846d5 96 // checking behavior level
bbbobbbieo 14:f43b386b8b5d 97 violence_level = int(TFC_GetDIP_Switch());
bbbobbbieo 14:f43b386b8b5d 98
bbergandy 25:1d0f586aaf0c 99 if (violence_level==3) {
bbergandy 25:1d0f586aaf0c 100 current_left_motor_speed = -(AGGRESSIVE);
bbergandy 25:1d0f586aaf0c 101 current_right_motor_speed = AGGRESSIVE;
bbbobbbieo 15:830209e846d5 102 }
bbergandy 25:1d0f586aaf0c 103 if (violence_level==2) {
bbergandy 25:1d0f586aaf0c 104 current_left_motor_speed = -(MODERATE);
bbergandy 25:1d0f586aaf0c 105 current_right_motor_speed = (MODERATE);
bbbobbbieo 23:fe15b6147c1b 106 }
bbbobbbieo 15:830209e846d5 107 else if (violence_level==1) {
bbergandy 25:1d0f586aaf0c 108 current_left_motor_speed = -(CONSERVATIVE);
bbergandy 25:1d0f586aaf0c 109 current_right_motor_speed = CONSERVATIVE;
bbbobbbieo 14:f43b386b8b5d 110 }
bbbobbbieo 15:830209e846d5 111 else if (violence_level==0) {
bbergandy 25:1d0f586aaf0c 112 current_left_motor_speed = STOP;
bbergandy 25:1d0f586aaf0c 113 current_right_motor_speed = STOP;
bbbobbbieo 15:830209e846d5 114 }
bbbobbbieo 15:830209e846d5 115 else {
bbergandy 25:1d0f586aaf0c 116 current_left_motor_speed = STOP;
bbergandy 25:1d0f586aaf0c 117 current_right_motor_speed = STOP;
bbbobbbieo 15:830209e846d5 118 }
bbbobbbieo 15:830209e846d5 119
bbbobbbieo 14:f43b386b8b5d 120
bbbobbbieo 15:830209e846d5 121 // protection block
bbbobbbieo 15:830209e846d5 122 if(current_left_motor_speed >= 0.5)
bbbobbbieo 15:830209e846d5 123 current_left_motor_speed= 0.5;
bbbobbbieo 15:830209e846d5 124 if(current_right_motor_speed >= 0.5)
bbbobbbieo 15:830209e846d5 125 current_right_motor_speed= 0.5;
bbbobbbieo 15:830209e846d5 126 if(current_left_motor_speed <= -0.5)
bbbobbbieo 15:830209e846d5 127 current_left_motor_speed= -0.5;
bbbobbbieo 15:830209e846d5 128 if(current_right_motor_speed <= -0.5)
bbbobbbieo 15:830209e846d5 129 current_right_motor_speed= -0.5;
bbbobbbieo 14:f43b386b8b5d 130
bbbobbbieo 14:f43b386b8b5d 131 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
bbbobbbieo 14:f43b386b8b5d 132 }// end motor enabled
bbbobbbieo 14:f43b386b8b5d 133 else {
bbbobbbieo 14:f43b386b8b5d 134 TFC_HBRIDGE_DISABLE;
bbbobbbieo 14:f43b386b8b5d 135 }// end motor disabled
bbbobbbieo 14:f43b386b8b5d 136
bbbobbbieo 15:830209e846d5 137 // camera stuff
bbbobbbieo 14:f43b386b8b5d 138 if (linescan_enable) {
bbbobbbieo 14:f43b386b8b5d 139 if (TFC_LineScanImageReady !=0) {
bbbobbbieo 14:f43b386b8b5d 140
bbbobbbieo 15:830209e846d5 141 if (linescan_ping_pong) {
bbbobbbieo 15:830209e846d5 142 //checking channel 0
bbbobbbieo 15:830209e846d5 143
bbbobbbieo 15:830209e846d5 144 //checking center pixel, displays aprox value on leds
bbbobbbieo 14:f43b386b8b5d 145 uint8_t shitnum = 1;
bbbobbbieo 14:f43b386b8b5d 146 if (*(TFC_LineScanImage0+64) > 800)
bbbobbbieo 14:f43b386b8b5d 147 shitnum = 15;
bbbobbbieo 14:f43b386b8b5d 148 else if((*(TFC_LineScanImage0+64) > 450))
bbbobbbieo 14:f43b386b8b5d 149 shitnum = 7;
bbbobbbieo 14:f43b386b8b5d 150 else if((*(TFC_LineScanImage0+64) > 400))
bbbobbbieo 14:f43b386b8b5d 151 shitnum = 3;
bbbobbbieo 14:f43b386b8b5d 152 else
bbbobbbieo 14:f43b386b8b5d 153 shitnum = 1;
bbbobbbieo 14:f43b386b8b5d 154 TFC_SetBatteryLED(shitnum);
bbbobbbieo 14:f43b386b8b5d 155
bbbobbbieo 15:830209e846d5 156
bbbobbbieo 15:830209e846d5 157 // checking for center line (single line)
bbbobbbieo 14:f43b386b8b5d 158 for (uint16_t i=0; i<128; i++) {
bbbobbbieo 14:f43b386b8b5d 159 if ((*(TFC_LineScanImage0+i) < 300)) {
bbbobbbieo 14:f43b386b8b5d 160 black_values_list[black_value_count] = i;
bbbobbbieo 14:f43b386b8b5d 161 black_value_count++;
bbbobbbieo 14:f43b386b8b5d 162 }
bbbobbbieo 14:f43b386b8b5d 163 }
bbbobbbieo 14:f43b386b8b5d 164
bbbobbbieo 14:f43b386b8b5d 165 for(int i=0; i<black_value_count; i++) {
bbbobbbieo 14:f43b386b8b5d 166 sum_black += black_values_list[i];
bbbobbbieo 14:f43b386b8b5d 167 }
bbbobbbieo 14:f43b386b8b5d 168
bbbobbbieo 20:c728b8ffad97 169 //update history
bbbobbbieo 20:c728b8ffad97 170 center_past_4= center_past_3;
bbbobbbieo 20:c728b8ffad97 171 center_past_3= center_past_2;
bbbobbbieo 20:c728b8ffad97 172 center_past_2= center_past_1;
bbbobbbieo 20:c728b8ffad97 173 center_past_1= center_now;
mperella 26:3f7ddc7d5bdf 174
mperella 28:06cefc8dd15e 175 for(int i = 49; i >= 0; i--)
mperella 26:3f7ddc7d5bdf 176 {
mperella 26:3f7ddc7d5bdf 177 if(i = 0)
mperella 26:3f7ddc7d5bdf 178 {
mperella 26:3f7ddc7d5bdf 179 centers_List[i] = center_now;
mperella 26:3f7ddc7d5bdf 180 }
mperella 26:3f7ddc7d5bdf 181 else
mperella 26:3f7ddc7d5bdf 182 {
mperella 26:3f7ddc7d5bdf 183 centers_List[i] = centers_List[i-1];
mperella 26:3f7ddc7d5bdf 184 }
mperella 28:06cefc8dd15e 185 }
bbbobbbieo 20:c728b8ffad97 186
bbbobbbieo 15:830209e846d5 187 // value of center of black (single line)
bbbobbbieo 20:c728b8ffad97 188 //black_center_value = sum_black / black_value_count;
bbbobbbieo 20:c728b8ffad97 189 center_now = sum_black / black_value_count;
bbbobbbieo 20:c728b8ffad97 190
bbbobbbieo 20:c728b8ffad97 191 // best guess of center based on weighted average of history
bbbobbbieo 24:504a58daa06e 192 //black_center_value = (5*center_now + 10*center_past_1 + 15*center_past_2 +30*center_past_3 +40*center_past_4)/100;
mperella 28:06cefc8dd15e 193 black_center_value = (5*center_now + 10*center_past_1 + 15*center_past_2 +30*center_past_3 +40*center_past_4)/100;
bbbobbbieo 15:830209e846d5 194
mperella 17:c643b6b4a96f 195 /* ******* PID ALGORITHM *******
mperella 17:c643b6b4a96f 196
mperella 17:c643b6b4a96f 197 error = set_point - black_center_value;
mperella 17:c643b6b4a96f 198 integral = integral + error*dt;
mperella 17:c643b6b4a96f 199 derivative = (error - previous_error)/dt;
mperella 17:c643b6b4a96f 200 output = kp*error + ki*integral + kd*derivative;
mperella 17:c643b6b4a96f 201 previous error = error;
mperella 17:c643b6b4a96f 202
mperella 17:c643b6b4a96f 203 GOTTA DO SOME TESTS/SIMULATIONS TO CALIBRATE SERVO ADJUSTMENTS
mperella 17:c643b6b4a96f 204
mperella 17:c643b6b4a96f 205
mperella 17:c643b6b4a96f 206 *****************************
mperella 17:c643b6b4a96f 207 */
bbbobbbieo 14:f43b386b8b5d 208
bbbobbbieo 15:830209e846d5 209 // need to turn left
mperella 28:06cefc8dd15e 210
mperella 28:06cefc8dd15e 211 if (black_center_value == BLACK_THRESHOLD)
mperella 28:06cefc8dd15e 212 {
mperella 28:06cefc8dd15e 213 TFC_SetServo(0,0);
mperella 28:06cefc8dd15e 214
mperella 29:91bda3a63f17 215 int temp = 0;
mperella 29:91bda3a63f17 216 int temp2 = 0;
mperella 28:06cefc8dd15e 217
mperella 28:06cefc8dd15e 218 for(int i = 20; i < 30; i++)
mperella 28:06cefc8dd15e 219 {
mperella 28:06cefc8dd15e 220 temp = centers_List[i] + temp;
mperella 28:06cefc8dd15e 221 }
mperella 28:06cefc8dd15e 222
mperella 28:06cefc8dd15e 223 for(int i = 30; i < 40; i++)
mperella 28:06cefc8dd15e 224 {
mperella 28:06cefc8dd15e 225 temp2 = centers_List[i] + temp2;
mperella 28:06cefc8dd15e 226 }
mperella 28:06cefc8dd15e 227
mperella 28:06cefc8dd15e 228 temp = temp/10;
mperella 28:06cefc8dd15e 229 temp2 = temp/10;
mperella 28:06cefc8dd15e 230
mperella 28:06cefc8dd15e 231 if(temp < 70 && temp > 60 && temp2 < 70 && temp2 > 60)
mperella 28:06cefc8dd15e 232 {
mperella 28:06cefc8dd15e 233 current_left_motor_speed = current_left_motor_speed + .010;
mperella 28:06cefc8dd15e 234 current_right_motor_speed = current_right_motor_speed + .010;
mperella 28:06cefc8dd15e 235 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
mperella 28:06cefc8dd15e 236 }
mperella 28:06cefc8dd15e 237 }
mperella 28:06cefc8dd15e 238
bbbobbbieo 15:830209e846d5 239 if (black_center_value < 64) {
bbbobbbieo 15:830209e846d5 240
bbbobbbieo 16:11ba5d6f42ba 241 current_servo_position= float(.01875*black_center_value-(1.2));
bbbobbbieo 14:f43b386b8b5d 242 if(current_servo_position <= -0.4)
bbbobbbieo 14:f43b386b8b5d 243 current_servo_position = -0.4;
bbbobbbieo 14:f43b386b8b5d 244 TFC_SetServo(0, current_servo_position);
mperella 26:3f7ddc7d5bdf 245
mperella 29:91bda3a63f17 246 bool listSame = true;
mperella 26:3f7ddc7d5bdf 247
mperella 29:91bda3a63f17 248 int temp = 0;
mperella 29:91bda3a63f17 249 int temp2 = 0;
mperella 28:06cefc8dd15e 250
mperella 28:06cefc8dd15e 251 for(int i = 20; i < 30; i++)
mperella 28:06cefc8dd15e 252 {
mperella 28:06cefc8dd15e 253 temp = centers_List[i] + temp;
mperella 28:06cefc8dd15e 254 }
mperella 28:06cefc8dd15e 255
mperella 28:06cefc8dd15e 256 for(int i = 30; i < 40; i++)
mperella 28:06cefc8dd15e 257 {
mperella 28:06cefc8dd15e 258 temp2 = centers_List[i] + temp2;
mperella 28:06cefc8dd15e 259 }
mperella 28:06cefc8dd15e 260
mperella 28:06cefc8dd15e 261 temp = temp/10;
mperella 28:06cefc8dd15e 262 temp2 = temp/10;
mperella 28:06cefc8dd15e 263
mperella 28:06cefc8dd15e 264 if(temp < 70 && temp > 60 && temp2 < 70 && temp2 > 60)
mperella 28:06cefc8dd15e 265 {
mperella 28:06cefc8dd15e 266 current_left_motor_speed = current_left_motor_speed + .010;
mperella 28:06cefc8dd15e 267 current_right_motor_speed = current_right_motor_speed + .010;
mperella 28:06cefc8dd15e 268 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
mperella 28:06cefc8dd15e 269 }
mperella 28:06cefc8dd15e 270
mperella 28:06cefc8dd15e 271
mperella 28:06cefc8dd15e 272
mperella 28:06cefc8dd15e 273
mperella 26:3f7ddc7d5bdf 274 /*
mperella 26:3f7ddc7d5bdf 275 if(center_now + 5 < center_past_4 && center_now - 5 > center_past_4)
mperella 26:3f7ddc7d5bdf 276 {
mperella 26:3f7ddc7d5bdf 277 accelList[lastAccessed] = 1;
mperella 26:3f7ddc7d5bdf 278 if(lastAccessed != 2)
mperella 26:3f7ddc7d5bdf 279 {
mperella 26:3f7ddc7d5bdf 280 lastAccessed = lastAccessed + 1;
mperella 26:3f7ddc7d5bdf 281 }
mperella 26:3f7ddc7d5bdf 282 else
mperella 26:3f7ddc7d5bdf 283 {
mperella 26:3f7ddc7d5bdf 284 lastAccessed = 0;
mperella 26:3f7ddc7d5bdf 285 }
mperella 26:3f7ddc7d5bdf 286 }
mperella 26:3f7ddc7d5bdf 287
mperella 26:3f7ddc7d5bdf 288
mperella 26:3f7ddc7d5bdf 289
mperella 26:3f7ddc7d5bdf 290 for(int i = 0; i < 3; i++)
mperella 26:3f7ddc7d5bdf 291 {
mperella 26:3f7ddc7d5bdf 292 while(listSame)
mperella 26:3f7ddc7d5bdf 293 {
mperella 26:3f7ddc7d5bdf 294 if(accelList[i] > 0)
mperella 26:3f7ddc7d5bdf 295 {
mperella 26:3f7ddc7d5bdf 296
mperella 26:3f7ddc7d5bdf 297 }
mperella 26:3f7ddc7d5bdf 298 else
mperella 26:3f7ddc7d5bdf 299 {
mperella 26:3f7ddc7d5bdf 300 listSame = false;
mperella 26:3f7ddc7d5bdf 301 }
mperella 26:3f7ddc7d5bdf 302 }
mperella 26:3f7ddc7d5bdf 303 }
mperella 26:3f7ddc7d5bdf 304
mperella 26:3f7ddc7d5bdf 305 if(listSame)
mperella 26:3f7ddc7d5bdf 306 {
mperella 26:3f7ddc7d5bdf 307 current_left_motor_speed = current_left_motor_speed + .010;
mperella 26:3f7ddc7d5bdf 308 current_right_motor_speed = current_right_motor_speed + .010;
mperella 26:3f7ddc7d5bdf 309 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
mperella 26:3f7ddc7d5bdf 310 }
mperella 26:3f7ddc7d5bdf 311
mperella 26:3f7ddc7d5bdf 312
mperella 26:3f7ddc7d5bdf 313 */
bbbobbbieo 15:830209e846d5 314
bbbobbbieo 16:11ba5d6f42ba 315 //current_left_motor_speed = current_left_motor_speed + float(64-black_center_value)*.0025;
bbbobbbieo 16:11ba5d6f42ba 316 //current_right_motor_speed = current_right_motor_speed + float(64-black_center_value)*.0025;
bbbobbbieo 16:11ba5d6f42ba 317 if (violence_level !=0){
bbbobbbieo 19:85eb7991e2ab 318 current_left_motor_speed = current_left_motor_speed + float(float(64-black_center_value)*.025);// kinda reverse this...
bbbobbbieo 16:11ba5d6f42ba 319 current_right_motor_speed = current_right_motor_speed + float(float(64-black_center_value)*.045);// push more forwards
bbbobbbieo 16:11ba5d6f42ba 320 }
bbbobbbieo 16:11ba5d6f42ba 321
bbbobbbieo 16:11ba5d6f42ba 322 // protection block
bbbobbbieo 16:11ba5d6f42ba 323 if(current_left_motor_speed >= 0.5)
bbbobbbieo 16:11ba5d6f42ba 324 current_left_motor_speed= 0.5;
bbbobbbieo 16:11ba5d6f42ba 325 if(current_right_motor_speed >= 0.5)
bbbobbbieo 16:11ba5d6f42ba 326 current_right_motor_speed= 0.5;
bbbobbbieo 16:11ba5d6f42ba 327 if(current_left_motor_speed <= -0.5)
bbbobbbieo 16:11ba5d6f42ba 328 current_left_motor_speed= -0.5;
bbbobbbieo 16:11ba5d6f42ba 329 if(current_right_motor_speed <= -0.5)
bbbobbbieo 16:11ba5d6f42ba 330 current_right_motor_speed= -0.5;
bbbobbbieo 16:11ba5d6f42ba 331
bbbobbbieo 14:f43b386b8b5d 332 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
bbbobbbieo 14:f43b386b8b5d 333
bbbobbbieo 7:455e7dd338ee 334 }
bbbobbbieo 14:f43b386b8b5d 335
bbbobbbieo 15:830209e846d5 336 // need to turn right
bbergandy 25:1d0f586aaf0c 337 if (black_center_value > BLACK_THRESHOLD) {
bbbobbbieo 14:f43b386b8b5d 338
bbbobbbieo 16:11ba5d6f42ba 339 current_servo_position= float(.01875*black_center_value-(1.2));
bbbobbbieo 14:f43b386b8b5d 340 if( current_servo_position >= +0.4)
bbbobbbieo 14:f43b386b8b5d 341 current_servo_position = +0.4;
bbbobbbieo 14:f43b386b8b5d 342 TFC_SetServo(0, current_servo_position);
mperella 26:3f7ddc7d5bdf 343
mperella 29:91bda3a63f17 344 bool listSame = true;
mperella 26:3f7ddc7d5bdf 345
mperella 29:91bda3a63f17 346 int temp = 0;
mperella 29:91bda3a63f17 347 int temp2 = 0;
mperella 28:06cefc8dd15e 348
mperella 28:06cefc8dd15e 349 for(int i = 20; i < 30; i++)
mperella 28:06cefc8dd15e 350 {
mperella 28:06cefc8dd15e 351 temp = centers_List[i] + temp;
mperella 28:06cefc8dd15e 352 }
mperella 28:06cefc8dd15e 353
mperella 28:06cefc8dd15e 354 for(int i = 30; i < 40; i++)
mperella 28:06cefc8dd15e 355 {
mperella 28:06cefc8dd15e 356 temp2 = centers_List[i] + temp2;
mperella 28:06cefc8dd15e 357 }
mperella 28:06cefc8dd15e 358
mperella 28:06cefc8dd15e 359 temp = temp/10;
mperella 28:06cefc8dd15e 360 temp2 = temp/10;
mperella 28:06cefc8dd15e 361
mperella 28:06cefc8dd15e 362 if(temp < 70 && temp > 60 && temp2 < 70 && temp2 > 60)
mperella 28:06cefc8dd15e 363 {
mperella 28:06cefc8dd15e 364 current_left_motor_speed = current_left_motor_speed + .010;
mperella 28:06cefc8dd15e 365 current_right_motor_speed = current_right_motor_speed + .010;
mperella 28:06cefc8dd15e 366 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
mperella 28:06cefc8dd15e 367 }
mperella 28:06cefc8dd15e 368
mperella 26:3f7ddc7d5bdf 369 /*
mperella 26:3f7ddc7d5bdf 370 if(center_now + 5 < center_past_4 && center_now - 5 > center_past_4)
mperella 26:3f7ddc7d5bdf 371 {
mperella 26:3f7ddc7d5bdf 372 accelList[lastAccessed] = 1;
mperella 26:3f7ddc7d5bdf 373 if(lastAccessed != 2)
mperella 26:3f7ddc7d5bdf 374 {
mperella 26:3f7ddc7d5bdf 375 lastAccessed = lastAccessed + 1;
mperella 26:3f7ddc7d5bdf 376 }
mperella 26:3f7ddc7d5bdf 377 else
mperella 26:3f7ddc7d5bdf 378 {
mperella 26:3f7ddc7d5bdf 379 lastAccessed = 0;
mperella 26:3f7ddc7d5bdf 380 }
mperella 26:3f7ddc7d5bdf 381 }
mperella 26:3f7ddc7d5bdf 382
mperella 26:3f7ddc7d5bdf 383
mperella 26:3f7ddc7d5bdf 384
mperella 26:3f7ddc7d5bdf 385 for(int i = 0; i < 3; i++)
mperella 26:3f7ddc7d5bdf 386 {
mperella 26:3f7ddc7d5bdf 387 while(listSame)
mperella 26:3f7ddc7d5bdf 388 {
mperella 26:3f7ddc7d5bdf 389 if(accelList[i] > 0)
mperella 26:3f7ddc7d5bdf 390 {
mperella 26:3f7ddc7d5bdf 391
mperella 26:3f7ddc7d5bdf 392 }
mperella 26:3f7ddc7d5bdf 393 else
mperella 26:3f7ddc7d5bdf 394 {
mperella 26:3f7ddc7d5bdf 395 listSame = false;
mperella 26:3f7ddc7d5bdf 396 }
mperella 26:3f7ddc7d5bdf 397 }
mperella 26:3f7ddc7d5bdf 398 }
mperella 26:3f7ddc7d5bdf 399
mperella 26:3f7ddc7d5bdf 400 if(listSame)
mperella 26:3f7ddc7d5bdf 401 {
mperella 26:3f7ddc7d5bdf 402 current_left_motor_speed = current_left_motor_speed + .010;
mperella 26:3f7ddc7d5bdf 403 current_right_motor_speed = current_right_motor_speed + .010;
mperella 26:3f7ddc7d5bdf 404 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
mperella 26:3f7ddc7d5bdf 405 }
mperella 26:3f7ddc7d5bdf 406
mperella 26:3f7ddc7d5bdf 407
mperella 26:3f7ddc7d5bdf 408 */
bbbobbbieo 15:830209e846d5 409
bbbobbbieo 16:11ba5d6f42ba 410 //current_left_motor_speed = current_left_motor_speed - float(black_center_value-64)*.0025;
bbbobbbieo 16:11ba5d6f42ba 411 //current_right_motor_speed = current_right_motor_speed - float(black_center_value-64)*.0025;
bbbobbbieo 16:11ba5d6f42ba 412 if (violence_level !=0){
bbbobbbieo 16:11ba5d6f42ba 413 current_left_motor_speed = current_left_motor_speed - float(float(black_center_value-64)*.045);// push more forwards
bbbobbbieo 19:85eb7991e2ab 414 current_right_motor_speed = current_right_motor_speed - float(float(black_center_value-64)*.025);// kinda reverse this...
bbbobbbieo 16:11ba5d6f42ba 415 }
bbbobbbieo 15:830209e846d5 416
bbbobbbieo 16:11ba5d6f42ba 417 // protection block
bbbobbbieo 16:11ba5d6f42ba 418 if(current_left_motor_speed >= 0.5)
bbbobbbieo 16:11ba5d6f42ba 419 current_left_motor_speed= 0.5;
bbbobbbieo 16:11ba5d6f42ba 420 if(current_right_motor_speed >= 0.5)
bbbobbbieo 16:11ba5d6f42ba 421 current_right_motor_speed= 0.5;
bbbobbbieo 16:11ba5d6f42ba 422 if(current_left_motor_speed <= -0.5)
bbbobbbieo 16:11ba5d6f42ba 423 current_left_motor_speed= -0.5;
bbbobbbieo 16:11ba5d6f42ba 424 if(current_right_motor_speed <= -0.5)
bbbobbbieo 16:11ba5d6f42ba 425 current_right_motor_speed= -0.5;
bbbobbbieo 16:11ba5d6f42ba 426
bbbobbbieo 14:f43b386b8b5d 427 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
bbbobbbieo 15:830209e846d5 428
bbbobbbieo 14:f43b386b8b5d 429 }
bbbobbbieo 14:f43b386b8b5d 430
bbbobbbieo 15:830209e846d5 431 // clearing values for next image processing round
bbbobbbieo 14:f43b386b8b5d 432 black_value_count = 0;
bbbobbbieo 14:f43b386b8b5d 433 black_center_value = 0;
bbbobbbieo 14:f43b386b8b5d 434 sum_black = 0;
bbbobbbieo 14:f43b386b8b5d 435
bbbobbbieo 15:830209e846d5 436 // end image processing
bbbobbbieo 14:f43b386b8b5d 437
bbbobbbieo 14:f43b386b8b5d 438 linescan_ping_pong = false;
bbbobbbieo 14:f43b386b8b5d 439 } // end checking channel 0
bbbobbbieo 15:830209e846d5 440
bbbobbbieo 15:830209e846d5 441 else { //checking channel 1
bbbobbbieo 14:f43b386b8b5d 442 linescan_ping_pong = true;
bbbobbbieo 14:f43b386b8b5d 443 }
bbbobbbieo 14:f43b386b8b5d 444
bbergandy 25:1d0f586aaf0c 445 TFC_LineScanImageReady = 0; // since we used it, we reset the flag
bbbobbbieo 14:f43b386b8b5d 446 }// end imageready
bbbobbbieo 14:f43b386b8b5d 447 }// end linescan stuff
bbbobbbieo 0:d57117b2188d 448 }
bbbobbbieo 0:d57117b2188d 449 }
bbbobbbieo 8:946806df7347 450
bbbobbbieo 8:946806df7347 451
bbbobbbieo 15:830209e846d5 452 // shit code down here