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@37:9c544f53563d, 2015-03-04 (annotated)

Committer:
mperella
Date:
Wed Mar 04 22:34:12 2015 +0000
Revision:
37:9c544f53563d
Parent:
36:3491a7d8ffb6
Child:
38:ecb6da94cb64
added serial code. hopefully should be able to use this to communicate with car.

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