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@38:ecb6da94cb64, 2015-03-05 (annotated)

Committer:
bbbobbbieo
Date:
Thu Mar 05 20:03:48 2015 +0000
Revision:
38:ecb6da94cb64
Parent:
37:9c544f53563d
Child:
39:4a0803d7575d
cubic turning, turn restriction, i guess it runs the track

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