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@33:e14ef9206a63, 2015-03-03 (annotated)

Committer:
bbergandy
Date:
Tue Mar 03 20:11:12 2015 +0000
Revision:
33:e14ef9206a63
Parent:
29:91bda3a63f17
Parent:
31:0c2b49175036
Child:
34:fac57f153973
nothing

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