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@36:3491a7d8ffb6, 2015-03-03 (annotated)

Committer:
bbergandy
Date:
Tue Mar 03 21:49:11 2015 +0000
Revision:
36:3491a7d8ffb6
Parent:
35:e1cfadfe7469
Child:
37:9c544f53563d
lot of stuff commented out to try to restore working car. seems to be mostly the same. gotta look through it

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 36:3491a7d8ffb6 8 #define BLACK_THRESHOLD 63
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
mperella 28:06cefc8dd15e 51 int center_now = 63;
mperella 28:06cefc8dd15e 52 int center_past_1 = 63;
mperella 28:06cefc8dd15e 53 int center_past_2 = 63;
mperella 28:06cefc8dd15e 54 int center_past_3 = 63;
mperella 28:06cefc8dd15e 55 int center_past_4 = 63;
bbbobbbieo 20:c728b8ffad97 56 //int best_guess_center = 64;
bbbobbbieo 20:c728b8ffad97 57
mperella 28:06cefc8dd15e 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 36:3491a7d8ffb6 106 else 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
bbergandy 36:3491a7d8ffb6 179 /*
mperella 28:06cefc8dd15e 180 for(int i = 49; i >= 0; i--)
mperella 26:3f7ddc7d5bdf 181 {
mperella 26:3f7ddc7d5bdf 182 if(i = 0)
mperella 26:3f7ddc7d5bdf 183 {
mperella 26:3f7ddc7d5bdf 184 centers_List[i] = center_now;
mperella 26:3f7ddc7d5bdf 185 }
mperella 26:3f7ddc7d5bdf 186 else
mperella 26:3f7ddc7d5bdf 187 {
mperella 26:3f7ddc7d5bdf 188 centers_List[i] = centers_List[i-1];
mperella 26:3f7ddc7d5bdf 189 }
bbergandy 36:3491a7d8ffb6 190 }
bbergandy 36:3491a7d8ffb6 191 */
bbbobbbieo 20:c728b8ffad97 192
bbbobbbieo 15:830209e846d5 193 // value of center of black (single line)
bbbobbbieo 20:c728b8ffad97 194 //black_center_value = sum_black / black_value_count;
bbbobbbieo 20:c728b8ffad97 195 center_now = sum_black / black_value_count;
bbbobbbieo 20:c728b8ffad97 196
bbbobbbieo 20:c728b8ffad97 197 // best guess of center based on weighted average of history
bbbobbbieo 24:504a58daa06e 198 //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 199 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 200
mperella 17:c643b6b4a96f 201 /* ******* PID ALGORITHM *******
mperella 17:c643b6b4a96f 202
mperella 17:c643b6b4a96f 203 error = set_point - black_center_value;
mperella 17:c643b6b4a96f 204 integral = integral + error*dt;
mperella 17:c643b6b4a96f 205 derivative = (error - previous_error)/dt;
mperella 17:c643b6b4a96f 206 output = kp*error + ki*integral + kd*derivative;
mperella 17:c643b6b4a96f 207 previous error = error;
mperella 17:c643b6b4a96f 208
mperella 17:c643b6b4a96f 209 GOTTA DO SOME TESTS/SIMULATIONS TO CALIBRATE SERVO ADJUSTMENTS
mperella 17:c643b6b4a96f 210
mperella 17:c643b6b4a96f 211
mperella 17:c643b6b4a96f 212 *****************************
mperella 17:c643b6b4a96f 213 */
mperella 28:06cefc8dd15e 214
bbergandy 36:3491a7d8ffb6 215 /*
mperella 28:06cefc8dd15e 216 if (black_center_value == BLACK_THRESHOLD)
mperella 28:06cefc8dd15e 217 {
mperella 28:06cefc8dd15e 218 TFC_SetServo(0,0);
mperella 28:06cefc8dd15e 219
mperella 29:91bda3a63f17 220 int temp = 0;
mperella 29:91bda3a63f17 221 int temp2 = 0;
mperella 28:06cefc8dd15e 222
mperella 28:06cefc8dd15e 223 for(int i = 20; i < 30; i++)
mperella 28:06cefc8dd15e 224 {
mperella 28:06cefc8dd15e 225 temp = centers_List[i] + temp;
mperella 28:06cefc8dd15e 226 }
mperella 28:06cefc8dd15e 227
mperella 28:06cefc8dd15e 228 for(int i = 30; i < 40; i++)
mperella 28:06cefc8dd15e 229 {
mperella 28:06cefc8dd15e 230 temp2 = centers_List[i] + temp2;
mperella 28:06cefc8dd15e 231 }
mperella 28:06cefc8dd15e 232
mperella 28:06cefc8dd15e 233 temp = temp/10;
bbergandy 35:e1cfadfe7469 234 temp2 = temp2/10;
mperella 28:06cefc8dd15e 235
bbergandy 35:e1cfadfe7469 236 if(temp < 73 && temp > 53 && temp2 < 73 && temp2 > 53)
mperella 28:06cefc8dd15e 237 {
bbergandy 35:e1cfadfe7469 238 current_left_motor_speed = current_left_motor_speed - .010;
mperella 28:06cefc8dd15e 239 current_right_motor_speed = current_right_motor_speed + .010;
mperella 28:06cefc8dd15e 240 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
mperella 28:06cefc8dd15e 241 }
bbergandy 36:3491a7d8ffb6 242
mperella 28:06cefc8dd15e 243 }
bbergandy 36:3491a7d8ffb6 244 */
mperella 28:06cefc8dd15e 245
bbergandy 31:0c2b49175036 246 if (black_center_value < BLACK_THRESHOLD) {
bbbobbbieo 15:830209e846d5 247
bbergandy 36:3491a7d8ffb6 248 //current_servo_position = float(.01875*black_center_value-(1.2));
bbergandy 36:3491a7d8ffb6 249 current_servo_position = float(.01875*center_now-(1.2));
bbbobbbieo 14:f43b386b8b5d 250 if(current_servo_position <= -0.4)
bbbobbbieo 14:f43b386b8b5d 251 current_servo_position = -0.4;
bbbobbbieo 14:f43b386b8b5d 252 TFC_SetServo(0, current_servo_position);
mperella 26:3f7ddc7d5bdf 253
bbergandy 36:3491a7d8ffb6 254 //bool listSame = true;
bbergandy 36:3491a7d8ffb6 255 /*
mperella 29:91bda3a63f17 256 int temp = 0;
mperella 29:91bda3a63f17 257 int temp2 = 0;
mperella 28:06cefc8dd15e 258
mperella 28:06cefc8dd15e 259 for(int i = 20; i < 30; i++)
mperella 28:06cefc8dd15e 260 {
mperella 28:06cefc8dd15e 261 temp = centers_List[i] + temp;
mperella 28:06cefc8dd15e 262 }
mperella 28:06cefc8dd15e 263
mperella 28:06cefc8dd15e 264 for(int i = 30; i < 40; i++)
mperella 28:06cefc8dd15e 265 {
mperella 28:06cefc8dd15e 266 temp2 = centers_List[i] + temp2;
mperella 28:06cefc8dd15e 267 }
mperella 28:06cefc8dd15e 268
mperella 28:06cefc8dd15e 269 temp = temp/10;
bbergandy 35:e1cfadfe7469 270 temp2 = temp2/10;
mperella 28:06cefc8dd15e 271
bbergandy 35:e1cfadfe7469 272 if(temp < 73 && temp > 53 && temp2 < 73 && temp2 > 53)
mperella 28:06cefc8dd15e 273 {
bbergandy 35:e1cfadfe7469 274 current_left_motor_speed = current_left_motor_speed - .010;
mperella 28:06cefc8dd15e 275 current_right_motor_speed = current_right_motor_speed + .010;
mperella 28:06cefc8dd15e 276 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
mperella 28:06cefc8dd15e 277 }
mperella 28:06cefc8dd15e 278
bbergandy 36:3491a7d8ffb6 279 */
mperella 28:06cefc8dd15e 280
mperella 28:06cefc8dd15e 281
mperella 26:3f7ddc7d5bdf 282 /*
mperella 26:3f7ddc7d5bdf 283 if(center_now + 5 < center_past_4 && center_now - 5 > center_past_4)
mperella 26:3f7ddc7d5bdf 284 {
mperella 26:3f7ddc7d5bdf 285 accelList[lastAccessed] = 1;
mperella 26:3f7ddc7d5bdf 286 if(lastAccessed != 2)
mperella 26:3f7ddc7d5bdf 287 {
mperella 26:3f7ddc7d5bdf 288 lastAccessed = lastAccessed + 1;
mperella 26:3f7ddc7d5bdf 289 }
mperella 26:3f7ddc7d5bdf 290 else
mperella 26:3f7ddc7d5bdf 291 {
mperella 26:3f7ddc7d5bdf 292 lastAccessed = 0;
mperella 26:3f7ddc7d5bdf 293 }
mperella 26:3f7ddc7d5bdf 294 }
mperella 26:3f7ddc7d5bdf 295
mperella 26:3f7ddc7d5bdf 296
mperella 26:3f7ddc7d5bdf 297
mperella 26:3f7ddc7d5bdf 298 for(int i = 0; i < 3; i++)
mperella 26:3f7ddc7d5bdf 299 {
mperella 26:3f7ddc7d5bdf 300 while(listSame)
mperella 26:3f7ddc7d5bdf 301 {
mperella 26:3f7ddc7d5bdf 302 if(accelList[i] > 0)
mperella 26:3f7ddc7d5bdf 303 {
mperella 26:3f7ddc7d5bdf 304
mperella 26:3f7ddc7d5bdf 305 }
mperella 26:3f7ddc7d5bdf 306 else
mperella 26:3f7ddc7d5bdf 307 {
mperella 26:3f7ddc7d5bdf 308 listSame = false;
mperella 26:3f7ddc7d5bdf 309 }
mperella 26:3f7ddc7d5bdf 310 }
mperella 26:3f7ddc7d5bdf 311 }
mperella 26:3f7ddc7d5bdf 312
mperella 26:3f7ddc7d5bdf 313 if(listSame)
mperella 26:3f7ddc7d5bdf 314 {
mperella 26:3f7ddc7d5bdf 315 current_left_motor_speed = current_left_motor_speed + .010;
mperella 26:3f7ddc7d5bdf 316 current_right_motor_speed = current_right_motor_speed + .010;
mperella 26:3f7ddc7d5bdf 317 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
mperella 26:3f7ddc7d5bdf 318 }
mperella 26:3f7ddc7d5bdf 319
mperella 26:3f7ddc7d5bdf 320
mperella 26:3f7ddc7d5bdf 321 */
bbbobbbieo 15:830209e846d5 322
bbbobbbieo 16:11ba5d6f42ba 323 //current_left_motor_speed = current_left_motor_speed + float(64-black_center_value)*.0025;
bbbobbbieo 16:11ba5d6f42ba 324 //current_right_motor_speed = current_right_motor_speed + float(64-black_center_value)*.0025;
bbbobbbieo 16:11ba5d6f42ba 325 if (violence_level !=0){
bbergandy 36:3491a7d8ffb6 326 current_left_motor_speed = current_left_motor_speed + float(float(63-black_center_value)*.025);// kinda reverse this...
bbergandy 36:3491a7d8ffb6 327 current_right_motor_speed = current_right_motor_speed + float(float(63-black_center_value)*.045);// push more forwards
bbbobbbieo 16:11ba5d6f42ba 328 }
bbbobbbieo 16:11ba5d6f42ba 329
bbbobbbieo 16:11ba5d6f42ba 330 // protection block
bbbobbbieo 16:11ba5d6f42ba 331 if(current_left_motor_speed >= 0.5)
bbbobbbieo 16:11ba5d6f42ba 332 current_left_motor_speed= 0.5;
bbbobbbieo 16:11ba5d6f42ba 333 if(current_right_motor_speed >= 0.5)
bbbobbbieo 16:11ba5d6f42ba 334 current_right_motor_speed= 0.5;
bbbobbbieo 16:11ba5d6f42ba 335 if(current_left_motor_speed <= -0.5)
bbbobbbieo 16:11ba5d6f42ba 336 current_left_motor_speed= -0.5;
bbbobbbieo 16:11ba5d6f42ba 337 if(current_right_motor_speed <= -0.5)
bbbobbbieo 16:11ba5d6f42ba 338 current_right_motor_speed= -0.5;
bbbobbbieo 16:11ba5d6f42ba 339
bbbobbbieo 14:f43b386b8b5d 340 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
bbbobbbieo 14:f43b386b8b5d 341
bbbobbbieo 7:455e7dd338ee 342 }
bbbobbbieo 15:830209e846d5 343 // need to turn right
bbergandy 31:0c2b49175036 344 else if (black_center_value > BLACK_THRESHOLD) {
bbbobbbieo 14:f43b386b8b5d 345
bbergandy 36:3491a7d8ffb6 346 //current_servo_position= float(.01875*black_center_value-(1.2));
bbergandy 36:3491a7d8ffb6 347 current_servo_position= float(.01875*center_now-(1.2));
bbergandy 36:3491a7d8ffb6 348 if( current_servo_position >= 0.4)
bbergandy 36:3491a7d8ffb6 349 current_servo_position = 0.4;
bbbobbbieo 14:f43b386b8b5d 350 TFC_SetServo(0, current_servo_position);
mperella 26:3f7ddc7d5bdf 351
bbergandy 36:3491a7d8ffb6 352 //bool listSame = true;
bbergandy 36:3491a7d8ffb6 353 /*
mperella 29:91bda3a63f17 354 int temp = 0;
mperella 29:91bda3a63f17 355 int temp2 = 0;
mperella 28:06cefc8dd15e 356
mperella 28:06cefc8dd15e 357 for(int i = 20; i < 30; i++)
mperella 28:06cefc8dd15e 358 {
mperella 28:06cefc8dd15e 359 temp = centers_List[i] + temp;
mperella 28:06cefc8dd15e 360 }
mperella 28:06cefc8dd15e 361
mperella 28:06cefc8dd15e 362 for(int i = 30; i < 40; i++)
mperella 28:06cefc8dd15e 363 {
mperella 28:06cefc8dd15e 364 temp2 = centers_List[i] + temp2;
mperella 28:06cefc8dd15e 365 }
mperella 28:06cefc8dd15e 366
mperella 28:06cefc8dd15e 367 temp = temp/10;
bbergandy 35:e1cfadfe7469 368 temp2 = temp2/10;
mperella 28:06cefc8dd15e 369
bbergandy 35:e1cfadfe7469 370 if(temp < 73 && temp > 53 && temp2 < 73 && temp2 > 53)
mperella 28:06cefc8dd15e 371 {
bbergandy 35:e1cfadfe7469 372 current_left_motor_speed = current_left_motor_speed - .010;
mperella 28:06cefc8dd15e 373 current_right_motor_speed = current_right_motor_speed + .010;
mperella 28:06cefc8dd15e 374 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
mperella 28:06cefc8dd15e 375 }
mperella 28:06cefc8dd15e 376
mperella 26:3f7ddc7d5bdf 377 /*
mperella 26:3f7ddc7d5bdf 378 if(center_now + 5 < center_past_4 && center_now - 5 > center_past_4)
mperella 26:3f7ddc7d5bdf 379 {
mperella 26:3f7ddc7d5bdf 380 accelList[lastAccessed] = 1;
mperella 26:3f7ddc7d5bdf 381 if(lastAccessed != 2)
mperella 26:3f7ddc7d5bdf 382 {
mperella 26:3f7ddc7d5bdf 383 lastAccessed = lastAccessed + 1;
mperella 26:3f7ddc7d5bdf 384 }
mperella 26:3f7ddc7d5bdf 385 else
mperella 26:3f7ddc7d5bdf 386 {
mperella 26:3f7ddc7d5bdf 387 lastAccessed = 0;
mperella 26:3f7ddc7d5bdf 388 }
mperella 26:3f7ddc7d5bdf 389 }
mperella 26:3f7ddc7d5bdf 390
mperella 26:3f7ddc7d5bdf 391
mperella 26:3f7ddc7d5bdf 392
mperella 26:3f7ddc7d5bdf 393 for(int i = 0; i < 3; i++)
mperella 26:3f7ddc7d5bdf 394 {
mperella 26:3f7ddc7d5bdf 395 while(listSame)
mperella 26:3f7ddc7d5bdf 396 {
mperella 26:3f7ddc7d5bdf 397 if(accelList[i] > 0)
mperella 26:3f7ddc7d5bdf 398 {
mperella 26:3f7ddc7d5bdf 399
mperella 26:3f7ddc7d5bdf 400 }
mperella 26:3f7ddc7d5bdf 401 else
mperella 26:3f7ddc7d5bdf 402 {
mperella 26:3f7ddc7d5bdf 403 listSame = false;
mperella 26:3f7ddc7d5bdf 404 }
mperella 26:3f7ddc7d5bdf 405 }
mperella 26:3f7ddc7d5bdf 406 }
mperella 26:3f7ddc7d5bdf 407
mperella 26:3f7ddc7d5bdf 408 if(listSame)
mperella 26:3f7ddc7d5bdf 409 {
mperella 26:3f7ddc7d5bdf 410 current_left_motor_speed = current_left_motor_speed + .010;
mperella 26:3f7ddc7d5bdf 411 current_right_motor_speed = current_right_motor_speed + .010;
mperella 26:3f7ddc7d5bdf 412 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
mperella 26:3f7ddc7d5bdf 413 }
mperella 26:3f7ddc7d5bdf 414
mperella 26:3f7ddc7d5bdf 415
mperella 26:3f7ddc7d5bdf 416 */
bbbobbbieo 15:830209e846d5 417
bbbobbbieo 16:11ba5d6f42ba 418 //current_left_motor_speed = current_left_motor_speed - float(black_center_value-64)*.0025;
bbbobbbieo 16:11ba5d6f42ba 419 //current_right_motor_speed = current_right_motor_speed - float(black_center_value-64)*.0025;
bbbobbbieo 16:11ba5d6f42ba 420 if (violence_level !=0){
bbergandy 36:3491a7d8ffb6 421 current_left_motor_speed = current_left_motor_speed - float(float(black_center_value-63)*.045);// push more forwards
bbergandy 36:3491a7d8ffb6 422 current_right_motor_speed = current_right_motor_speed - float(float(black_center_value-63)*.025);// kinda reverse this...
bbbobbbieo 16:11ba5d6f42ba 423 }
bbbobbbieo 15:830209e846d5 424
bbbobbbieo 16:11ba5d6f42ba 425 // protection block
bbbobbbieo 16:11ba5d6f42ba 426 if(current_left_motor_speed >= 0.5)
bbbobbbieo 16:11ba5d6f42ba 427 current_left_motor_speed= 0.5;
bbbobbbieo 16:11ba5d6f42ba 428 if(current_right_motor_speed >= 0.5)
bbbobbbieo 16:11ba5d6f42ba 429 current_right_motor_speed= 0.5;
bbbobbbieo 16:11ba5d6f42ba 430 if(current_left_motor_speed <= -0.5)
bbbobbbieo 16:11ba5d6f42ba 431 current_left_motor_speed= -0.5;
bbbobbbieo 16:11ba5d6f42ba 432 if(current_right_motor_speed <= -0.5)
bbbobbbieo 16:11ba5d6f42ba 433 current_right_motor_speed= -0.5;
bbbobbbieo 16:11ba5d6f42ba 434
bbbobbbieo 14:f43b386b8b5d 435 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
bbbobbbieo 15:830209e846d5 436
bbbobbbieo 14:f43b386b8b5d 437 }
bbbobbbieo 14:f43b386b8b5d 438
bbbobbbieo 15:830209e846d5 439 // clearing values for next image processing round
bbbobbbieo 14:f43b386b8b5d 440 black_value_count = 0;
bbbobbbieo 14:f43b386b8b5d 441 black_center_value = 0;
bbbobbbieo 14:f43b386b8b5d 442 sum_black = 0;
bbbobbbieo 14:f43b386b8b5d 443
bbbobbbieo 15:830209e846d5 444 // end image processing
bbbobbbieo 14:f43b386b8b5d 445
bbbobbbieo 14:f43b386b8b5d 446 linescan_ping_pong = false;
bbbobbbieo 14:f43b386b8b5d 447 } // end checking channel 0
bbbobbbieo 15:830209e846d5 448
bbbobbbieo 15:830209e846d5 449 else { //checking channel 1
bbbobbbieo 14:f43b386b8b5d 450 linescan_ping_pong = true;
bbbobbbieo 14:f43b386b8b5d 451 }
bbbobbbieo 14:f43b386b8b5d 452
bbergandy 25:1d0f586aaf0c 453 TFC_LineScanImageReady = 0; // since we used it, we reset the flag
bbbobbbieo 14:f43b386b8b5d 454 }// end imageready
bbbobbbieo 14:f43b386b8b5d 455 }// end linescan stuff
bbbobbbieo 0:d57117b2188d 456 }
bbbobbbieo 0:d57117b2188d 457 }
bbbobbbieo 8:946806df7347 458
bbbobbbieo 8:946806df7347 459
bbbobbbieo 15:830209e846d5 460 // shit code down here