fatboyslim / Mbed 2 deprecated bouncesinglecam

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

asof3_31_2015_1200h.txt@37:56dc8c2e8cf9, 2015-03-31 (annotated)

Committer:
bbbobbbieo
Date:
Tue Mar 31 15:50:34 2015 +0000
Revision:
37:56dc8c2e8cf9
31st at 1200h made it around 5 or 6 laps hoora

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bbbobbbieo 37:56dc8c2e8cf9 1 //#include "mbed.h"
bbbobbbieo 37:56dc8c2e8cf9 2 #include "TFC.h"
bbbobbbieo 37:56dc8c2e8cf9 3 #include <iostream>
bbbobbbieo 37:56dc8c2e8cf9 4 #include <stdio.h>
bbbobbbieo 37:56dc8c2e8cf9 5 //#include "serialib.h"
bbbobbbieo 37:56dc8c2e8cf9 6
bbbobbbieo 37:56dc8c2e8cf9 7 const float AGGRESSIVE2 = .43;
bbbobbbieo 37:56dc8c2e8cf9 8 const float AGGRESSIVE = .40;
bbbobbbieo 37:56dc8c2e8cf9 9 const float MODERATE =.35;
bbbobbbieo 37:56dc8c2e8cf9 10 const float CONSERVATIVE =.30;
bbbobbbieo 37:56dc8c2e8cf9 11 const float STOP =0;
bbbobbbieo 37:56dc8c2e8cf9 12 const float PROTECTION_THRESHOLD_UPPER =.7;
bbbobbbieo 37:56dc8c2e8cf9 13 const float PROTECTION_THRESHOLD_LOWER =-.7;
bbbobbbieo 37:56dc8c2e8cf9 14 const float TURN_FORWARD_ACCEL =0.045;
bbbobbbieo 37:56dc8c2e8cf9 15 const float TURN_BACKWARD_ACCEL =0.025;
bbbobbbieo 37:56dc8c2e8cf9 16 const float SERVO_CAN_MOVE_IN_ONE_FRAME =0.1;
bbbobbbieo 37:56dc8c2e8cf9 17 const float SERVO_MAX =.5;
bbbobbbieo 37:56dc8c2e8cf9 18
bbbobbbieo 37:56dc8c2e8cf9 19 const int BLACK_THRESHOLD =63;
bbbobbbieo 37:56dc8c2e8cf9 20 const int LINE_SCAN_LENGTH =128;
bbbobbbieo 37:56dc8c2e8cf9 21
bbbobbbieo 37:56dc8c2e8cf9 22
bbbobbbieo 37:56dc8c2e8cf9 23 DigitalOut myled(LED1);
bbbobbbieo 37:56dc8c2e8cf9 24
bbbobbbieo 37:56dc8c2e8cf9 25 int main()
bbbobbbieo 37:56dc8c2e8cf9 26 {
bbbobbbieo 37:56dc8c2e8cf9 27 //run this before anything
bbbobbbieo 37:56dc8c2e8cf9 28 TFC_Init();
bbbobbbieo 37:56dc8c2e8cf9 29
bbbobbbieo 37:56dc8c2e8cf9 30 //variables
bbbobbbieo 37:56dc8c2e8cf9 31 float current_servo_position = 0;
bbbobbbieo 37:56dc8c2e8cf9 32 float current_left_motor_speed = 0;
bbbobbbieo 37:56dc8c2e8cf9 33 float current_right_motor_speed = 0;
bbbobbbieo 37:56dc8c2e8cf9 34
bbbobbbieo 37:56dc8c2e8cf9 35 // gains on prop, int, der
bbbobbbieo 37:56dc8c2e8cf9 36 // subject to change, need to fine tune
bbbobbbieo 37:56dc8c2e8cf9 37
bbbobbbieo 37:56dc8c2e8cf9 38 bool rear_motor_enable_flag = true;
bbbobbbieo 37:56dc8c2e8cf9 39 bool linescan_ping_pong = false;
bbbobbbieo 37:56dc8c2e8cf9 40 bool linescan_enable = true;
bbbobbbieo 37:56dc8c2e8cf9 41
bbbobbbieo 37:56dc8c2e8cf9 42 bool whiteflag = false;
bbbobbbieo 37:56dc8c2e8cf9 43 bool blackflag = false;
bbbobbbieo 37:56dc8c2e8cf9 44 bool stopflag = false;
bbbobbbieo 37:56dc8c2e8cf9 45
bbbobbbieo 37:56dc8c2e8cf9 46 int black_list_one = 0;
bbbobbbieo 37:56dc8c2e8cf9 47 int white_list = 0;
bbbobbbieo 37:56dc8c2e8cf9 48 int black_list_two = 0;
bbbobbbieo 37:56dc8c2e8cf9 49
bbbobbbieo 37:56dc8c2e8cf9 50 int black_values_list[LINE_SCAN_LENGTH];
bbbobbbieo 37:56dc8c2e8cf9 51 int black_value_count = 0;
bbbobbbieo 37:56dc8c2e8cf9 52 int black_value_count_previous = 0;
bbbobbbieo 37:56dc8c2e8cf9 53 int black_center_value = 0;
bbbobbbieo 37:56dc8c2e8cf9 54 int sum_black = 0;
bbbobbbieo 37:56dc8c2e8cf9 55 int violence_level = 0;
bbbobbbieo 37:56dc8c2e8cf9 56
bbbobbbieo 37:56dc8c2e8cf9 57 int center_now = 63;
bbbobbbieo 37:56dc8c2e8cf9 58 int center_past_1 = 63;
bbbobbbieo 37:56dc8c2e8cf9 59 int center_past_2 = 63;
bbbobbbieo 37:56dc8c2e8cf9 60 int center_past_3 = 63;
bbbobbbieo 37:56dc8c2e8cf9 61 int center_past_4 = 63;
bbbobbbieo 37:56dc8c2e8cf9 62
bbbobbbieo 37:56dc8c2e8cf9 63 float left_counter =0;
bbbobbbieo 37:56dc8c2e8cf9 64 float right_counter =0;
bbbobbbieo 37:56dc8c2e8cf9 65 bool turn_left=false;
bbbobbbieo 37:56dc8c2e8cf9 66 bool turn_right=false;
bbbobbbieo 37:56dc8c2e8cf9 67
bbbobbbieo 37:56dc8c2e8cf9 68 float bullshit_offset = .08;
bbbobbbieo 37:56dc8c2e8cf9 69
bbbobbbieo 37:56dc8c2e8cf9 70 int num_of_straight =0;
bbbobbbieo 37:56dc8c2e8cf9 71 int num_of_left =0;
bbbobbbieo 37:56dc8c2e8cf9 72 int num_of_right=0;
bbbobbbieo 37:56dc8c2e8cf9 73
bbbobbbieo 37:56dc8c2e8cf9 74 int values_list_add =0;
bbbobbbieo 37:56dc8c2e8cf9 75 int value_count =0;
bbbobbbieo 37:56dc8c2e8cf9 76 int avg_value=0;
bbbobbbieo 37:56dc8c2e8cf9 77 //int black_threshhold=0;
bbbobbbieo 37:56dc8c2e8cf9 78 int black_threshhold =450;
bbbobbbieo 37:56dc8c2e8cf9 79
bbbobbbieo 37:56dc8c2e8cf9 80 // major loop
bbbobbbieo 37:56dc8c2e8cf9 81 while(1) {
bbbobbbieo 37:56dc8c2e8cf9 82
bbbobbbieo 37:56dc8c2e8cf9 83 // initial motor stuff
bbbobbbieo 37:56dc8c2e8cf9 84 if(rear_motor_enable_flag) {
bbbobbbieo 37:56dc8c2e8cf9 85 TFC_HBRIDGE_ENABLE;
bbbobbbieo 37:56dc8c2e8cf9 86
bbbobbbieo 37:56dc8c2e8cf9 87 // checking behavior level
bbbobbbieo 37:56dc8c2e8cf9 88 violence_level = int(TFC_GetDIP_Switch());
bbbobbbieo 37:56dc8c2e8cf9 89
bbbobbbieo 37:56dc8c2e8cf9 90
bbbobbbieo 37:56dc8c2e8cf9 91 if (violence_level==4) {
bbbobbbieo 37:56dc8c2e8cf9 92 current_left_motor_speed = -(AGGRESSIVE2);
bbbobbbieo 37:56dc8c2e8cf9 93 current_right_motor_speed = AGGRESSIVE2;
bbbobbbieo 37:56dc8c2e8cf9 94 }
bbbobbbieo 37:56dc8c2e8cf9 95 if (violence_level==3) {
bbbobbbieo 37:56dc8c2e8cf9 96 current_left_motor_speed = -(AGGRESSIVE);
bbbobbbieo 37:56dc8c2e8cf9 97 current_right_motor_speed = AGGRESSIVE;
bbbobbbieo 37:56dc8c2e8cf9 98 }
bbbobbbieo 37:56dc8c2e8cf9 99 else if (violence_level==2) {
bbbobbbieo 37:56dc8c2e8cf9 100 current_left_motor_speed = -(MODERATE);
bbbobbbieo 37:56dc8c2e8cf9 101 current_right_motor_speed = (MODERATE);
bbbobbbieo 37:56dc8c2e8cf9 102 }
bbbobbbieo 37:56dc8c2e8cf9 103 else if (violence_level==1) {
bbbobbbieo 37:56dc8c2e8cf9 104 current_left_motor_speed = -(CONSERVATIVE);
bbbobbbieo 37:56dc8c2e8cf9 105 current_right_motor_speed = CONSERVATIVE;
bbbobbbieo 37:56dc8c2e8cf9 106 }
bbbobbbieo 37:56dc8c2e8cf9 107 else if (violence_level==0) {
bbbobbbieo 37:56dc8c2e8cf9 108 current_left_motor_speed = STOP;
bbbobbbieo 37:56dc8c2e8cf9 109 current_right_motor_speed = STOP;
bbbobbbieo 37:56dc8c2e8cf9 110 }
bbbobbbieo 37:56dc8c2e8cf9 111 else {
bbbobbbieo 37:56dc8c2e8cf9 112 current_left_motor_speed = STOP;
bbbobbbieo 37:56dc8c2e8cf9 113 current_right_motor_speed = STOP;
bbbobbbieo 37:56dc8c2e8cf9 114 }
bbbobbbieo 37:56dc8c2e8cf9 115
bbbobbbieo 37:56dc8c2e8cf9 116 // protection block
bbbobbbieo 37:56dc8c2e8cf9 117 if(current_left_motor_speed >= PROTECTION_THRESHOLD_UPPER)
bbbobbbieo 37:56dc8c2e8cf9 118 current_left_motor_speed= PROTECTION_THRESHOLD_UPPER;
bbbobbbieo 37:56dc8c2e8cf9 119 if(current_right_motor_speed >= PROTECTION_THRESHOLD_UPPER)
bbbobbbieo 37:56dc8c2e8cf9 120 current_right_motor_speed = PROTECTION_THRESHOLD_UPPER;
bbbobbbieo 37:56dc8c2e8cf9 121 if(current_left_motor_speed <= PROTECTION_THRESHOLD_LOWER)
bbbobbbieo 37:56dc8c2e8cf9 122 current_left_motor_speed = PROTECTION_THRESHOLD_LOWER;
bbbobbbieo 37:56dc8c2e8cf9 123 if(current_right_motor_speed <= PROTECTION_THRESHOLD_LOWER)
bbbobbbieo 37:56dc8c2e8cf9 124 current_right_motor_speed = PROTECTION_THRESHOLD_LOWER;
bbbobbbieo 37:56dc8c2e8cf9 125
bbbobbbieo 37:56dc8c2e8cf9 126 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
bbbobbbieo 37:56dc8c2e8cf9 127 }// end motor enabled
bbbobbbieo 37:56dc8c2e8cf9 128 else {
bbbobbbieo 37:56dc8c2e8cf9 129 TFC_HBRIDGE_DISABLE;
bbbobbbieo 37:56dc8c2e8cf9 130 }// end motor disabled
bbbobbbieo 37:56dc8c2e8cf9 131
bbbobbbieo 37:56dc8c2e8cf9 132 // camera stuff
bbbobbbieo 37:56dc8c2e8cf9 133 if (linescan_enable) {
bbbobbbieo 37:56dc8c2e8cf9 134 if (TFC_LineScanImageReady !=0) {
bbbobbbieo 37:56dc8c2e8cf9 135
bbbobbbieo 37:56dc8c2e8cf9 136 if (linescan_ping_pong) {
bbbobbbieo 37:56dc8c2e8cf9 137 //checking channel 0
bbbobbbieo 37:56dc8c2e8cf9 138
bbbobbbieo 37:56dc8c2e8cf9 139
bbbobbbieo 37:56dc8c2e8cf9 140 // checking for center line (single line)
bbbobbbieo 37:56dc8c2e8cf9 141 for (uint16_t i=10; i<118; i++) {
bbbobbbieo 37:56dc8c2e8cf9 142
bbbobbbieo 37:56dc8c2e8cf9 143 int black_threshhold =600*(TFC_ReadPot(0));
bbbobbbieo 37:56dc8c2e8cf9 144 if ((*(TFC_LineScanImage0+i) < black_threshhold)) {
bbbobbbieo 37:56dc8c2e8cf9 145 black_values_list[black_value_count] = i;
bbbobbbieo 37:56dc8c2e8cf9 146 black_value_count++;
bbbobbbieo 37:56dc8c2e8cf9 147 //black_list_one++;
bbbobbbieo 37:56dc8c2e8cf9 148 }
bbbobbbieo 37:56dc8c2e8cf9 149 /*
bbbobbbieo 37:56dc8c2e8cf9 150 else
bbbobbbieo 37:56dc8c2e8cf9 151 {
bbbobbbieo 37:56dc8c2e8cf9 152 black_list_one = 0;
bbbobbbieo 37:56dc8c2e8cf9 153 }
bbbobbbieo 37:56dc8c2e8cf9 154
bbbobbbieo 37:56dc8c2e8cf9 155 if(black_list_one > 5)
bbbobbbieo 37:56dc8c2e8cf9 156 {
bbbobbbieo 37:56dc8c2e8cf9 157 for(uint16_t j = i+black_list_one; j<118; j++)
bbbobbbieo 37:56dc8c2e8cf9 158 {
bbbobbbieo 37:56dc8c2e8cf9 159 if((*(TFC_LineScanImage0+j) > black_threshhold))
bbbobbbieo 37:56dc8c2e8cf9 160 {
bbbobbbieo 37:56dc8c2e8cf9 161 white_list++;
bbbobbbieo 37:56dc8c2e8cf9 162 whiteflag = true;
bbbobbbieo 37:56dc8c2e8cf9 163 }
bbbobbbieo 37:56dc8c2e8cf9 164 else
bbbobbbieo 37:56dc8c2e8cf9 165 {
bbbobbbieo 37:56dc8c2e8cf9 166 whiteflag = false;
bbbobbbieo 37:56dc8c2e8cf9 167 }
bbbobbbieo 37:56dc8c2e8cf9 168 if(!whiteflag)
bbbobbbieo 37:56dc8c2e8cf9 169 {
bbbobbbieo 37:56dc8c2e8cf9 170 j = 200;
bbbobbbieo 37:56dc8c2e8cf9 171 }
bbbobbbieo 37:56dc8c2e8cf9 172 else if(whiteflag and white_list > 4)
bbbobbbieo 37:56dc8c2e8cf9 173 {
bbbobbbieo 37:56dc8c2e8cf9 174 for(uint16_t k = j+white_list; k < 118; k++)
bbbobbbieo 37:56dc8c2e8cf9 175 {
bbbobbbieo 37:56dc8c2e8cf9 176 if((*(TFC_LineScanImage0+k) < black_threshhold))
bbbobbbieo 37:56dc8c2e8cf9 177 {
bbbobbbieo 37:56dc8c2e8cf9 178 black_list_two++;
bbbobbbieo 37:56dc8c2e8cf9 179 blackflag = true;
bbbobbbieo 37:56dc8c2e8cf9 180 }
bbbobbbieo 37:56dc8c2e8cf9 181 else
bbbobbbieo 37:56dc8c2e8cf9 182 {
bbbobbbieo 37:56dc8c2e8cf9 183 blackflag = false;
bbbobbbieo 37:56dc8c2e8cf9 184 }
bbbobbbieo 37:56dc8c2e8cf9 185 if(!blackflag)
bbbobbbieo 37:56dc8c2e8cf9 186 {
bbbobbbieo 37:56dc8c2e8cf9 187 k = 200;
bbbobbbieo 37:56dc8c2e8cf9 188 }
bbbobbbieo 37:56dc8c2e8cf9 189 else if(blackflag and black_list_two > 3)
bbbobbbieo 37:56dc8c2e8cf9 190 {
bbbobbbieo 37:56dc8c2e8cf9 191 stopflag = true;
bbbobbbieo 37:56dc8c2e8cf9 192 }
bbbobbbieo 37:56dc8c2e8cf9 193 }
bbbobbbieo 37:56dc8c2e8cf9 194 }
bbbobbbieo 37:56dc8c2e8cf9 195 }
bbbobbbieo 37:56dc8c2e8cf9 196 }
bbbobbbieo 37:56dc8c2e8cf9 197 */
bbbobbbieo 37:56dc8c2e8cf9 198
bbbobbbieo 37:56dc8c2e8cf9 199 }
bbbobbbieo 37:56dc8c2e8cf9 200
bbbobbbieo 37:56dc8c2e8cf9 201 for(int i=0; i<black_value_count; i++) {
bbbobbbieo 37:56dc8c2e8cf9 202 sum_black += black_values_list[i];
bbbobbbieo 37:56dc8c2e8cf9 203 }
bbbobbbieo 37:56dc8c2e8cf9 204
bbbobbbieo 37:56dc8c2e8cf9 205 //update history
bbbobbbieo 37:56dc8c2e8cf9 206 /*
bbbobbbieo 37:56dc8c2e8cf9 207 center_past_4= center_past_3;
bbbobbbieo 37:56dc8c2e8cf9 208 center_past_3= center_past_2;
bbbobbbieo 37:56dc8c2e8cf9 209 center_past_2= center_past_1;
bbbobbbieo 37:56dc8c2e8cf9 210 center_past_1= center_now;
bbbobbbieo 37:56dc8c2e8cf9 211 */
bbbobbbieo 37:56dc8c2e8cf9 212
bbbobbbieo 37:56dc8c2e8cf9 213 black_value_count_previous = black_value_count;
bbbobbbieo 37:56dc8c2e8cf9 214
bbbobbbieo 37:56dc8c2e8cf9 215 //if (black_value_count>2)
bbbobbbieo 37:56dc8c2e8cf9 216 center_now = sum_black / black_value_count;
bbbobbbieo 37:56dc8c2e8cf9 217
bbbobbbieo 37:56dc8c2e8cf9 218 uint8_t num = 0;
bbbobbbieo 37:56dc8c2e8cf9 219
bbbobbbieo 37:56dc8c2e8cf9 220 if(center_now > 10 && center_now < 19) // turn right little
bbbobbbieo 37:56dc8c2e8cf9 221 num = 1;
bbbobbbieo 37:56dc8c2e8cf9 222 else if(center_now >= 19 && center_now < 39)// moderate right
bbbobbbieo 37:56dc8c2e8cf9 223 num = 3;
bbbobbbieo 37:56dc8c2e8cf9 224 else if(center_now >= 39 && center_now < 62)// snap right
bbbobbbieo 37:56dc8c2e8cf9 225 num = 2;
bbbobbbieo 37:56dc8c2e8cf9 226 else if(center_now >= 62 && center_now < 65) // center do nothing
bbbobbbieo 37:56dc8c2e8cf9 227 num = 15;
bbbobbbieo 37:56dc8c2e8cf9 228 else if(center_now >= 65 && center_now < 89)// snap left
bbbobbbieo 37:56dc8c2e8cf9 229 num = 4;
bbbobbbieo 37:56dc8c2e8cf9 230 else if(center_now >=89 && center_now < 109) // moderate left
bbbobbbieo 37:56dc8c2e8cf9 231 num = 12;
bbbobbbieo 37:56dc8c2e8cf9 232 else if(center_now >= 109 && center_now < 118) // turn left little
bbbobbbieo 37:56dc8c2e8cf9 233 num = 8;
bbbobbbieo 37:56dc8c2e8cf9 234
bbbobbbieo 37:56dc8c2e8cf9 235 else
bbbobbbieo 37:56dc8c2e8cf9 236 num = 0;
bbbobbbieo 37:56dc8c2e8cf9 237
bbbobbbieo 37:56dc8c2e8cf9 238 if (black_value_count<2)
bbbobbbieo 37:56dc8c2e8cf9 239 num = 0;
bbbobbbieo 37:56dc8c2e8cf9 240
bbbobbbieo 37:56dc8c2e8cf9 241 //if (black_value_count>26 and ((*(TFC_LineScanImage0+64) > 450) or (*(TFC_LineScanImage0+63) > 450)or(*(TFC_LineScanImage0+62) > 450)))
bbbobbbieo 37:56dc8c2e8cf9 242 //if (black_value_count + black_value_count_previous > 30 && (black_value_count > 15 or black_value_count_previous > 15) )
bbbobbbieo 37:56dc8c2e8cf9 243 // if(black_value_count > 22)
bbbobbbieo 37:56dc8c2e8cf9 244 //if (stopflag)
bbbobbbieo 37:56dc8c2e8cf9 245 if(0)
bbbobbbieo 37:56dc8c2e8cf9 246 {
bbbobbbieo 37:56dc8c2e8cf9 247 while(1)
bbbobbbieo 37:56dc8c2e8cf9 248 TFC_SetMotorPWM(0, 0);
bbbobbbieo 37:56dc8c2e8cf9 249 }
bbbobbbieo 37:56dc8c2e8cf9 250
bbbobbbieo 37:56dc8c2e8cf9 251 TFC_SetBatteryLED(num);
bbbobbbieo 37:56dc8c2e8cf9 252
bbbobbbieo 37:56dc8c2e8cf9 253 // best guess of center based on weighted average of history
bbbobbbieo 37:56dc8c2e8cf9 254 black_center_value = center_now;
bbbobbbieo 37:56dc8c2e8cf9 255
bbbobbbieo 37:56dc8c2e8cf9 256
bbbobbbieo 37:56dc8c2e8cf9 257 // turn left
bbbobbbieo 37:56dc8c2e8cf9 258 if (num==8 and right_counter <.35)
bbbobbbieo 37:56dc8c2e8cf9 259 {
bbbobbbieo 37:56dc8c2e8cf9 260 /*
bbbobbbieo 37:56dc8c2e8cf9 261 if (center_now <113)
bbbobbbieo 37:56dc8c2e8cf9 262 left_counter=-0.4;
bbbobbbieo 37:56dc8c2e8cf9 263 else
bbbobbbieo 37:56dc8c2e8cf9 264 {
bbbobbbieo 37:56dc8c2e8cf9 265 if (left_counter > -.15)
bbbobbbieo 37:56dc8c2e8cf9 266 left_counter=-0.15; // less drastic for outside parts
bbbobbbieo 37:56dc8c2e8cf9 267 }
bbbobbbieo 37:56dc8c2e8cf9 268 turn_left=true;
bbbobbbieo 37:56dc8c2e8cf9 269 turn_right=false;
bbbobbbieo 37:56dc8c2e8cf9 270 */
bbbobbbieo 37:56dc8c2e8cf9 271
bbbobbbieo 37:56dc8c2e8cf9 272
bbbobbbieo 37:56dc8c2e8cf9 273
bbbobbbieo 37:56dc8c2e8cf9 274 if (left_counter > -.15)
bbbobbbieo 37:56dc8c2e8cf9 275 left_counter=-0.15; // less drastic for outside parts
bbbobbbieo 37:56dc8c2e8cf9 276
bbbobbbieo 37:56dc8c2e8cf9 277 turn_left=true;
bbbobbbieo 37:56dc8c2e8cf9 278 turn_right=false;
bbbobbbieo 37:56dc8c2e8cf9 279
bbbobbbieo 37:56dc8c2e8cf9 280
bbbobbbieo 37:56dc8c2e8cf9 281 }
bbbobbbieo 37:56dc8c2e8cf9 282 else if(num == 12 and right_counter <.35)
bbbobbbieo 37:56dc8c2e8cf9 283 {
bbbobbbieo 37:56dc8c2e8cf9 284 if(left_counter > -.4)
bbbobbbieo 37:56dc8c2e8cf9 285 left_counter=-0.4;
bbbobbbieo 37:56dc8c2e8cf9 286 turn_left = true;
bbbobbbieo 37:56dc8c2e8cf9 287 turn_right = false;
bbbobbbieo 37:56dc8c2e8cf9 288 }
bbbobbbieo 37:56dc8c2e8cf9 289
bbbobbbieo 37:56dc8c2e8cf9 290 else if (num==4 and right_counter <.45)
bbbobbbieo 37:56dc8c2e8cf9 291 {
bbbobbbieo 37:56dc8c2e8cf9 292 left_counter=-0.6;
bbbobbbieo 37:56dc8c2e8cf9 293 //left_counter=-0.55;
bbbobbbieo 37:56dc8c2e8cf9 294 turn_left=true;
bbbobbbieo 37:56dc8c2e8cf9 295 turn_right=false;
bbbobbbieo 37:56dc8c2e8cf9 296 }
bbbobbbieo 37:56dc8c2e8cf9 297
bbbobbbieo 37:56dc8c2e8cf9 298 // need to turn right
bbbobbbieo 37:56dc8c2e8cf9 299 else if (num==1 and left_counter >-.35)
bbbobbbieo 37:56dc8c2e8cf9 300 {
bbbobbbieo 37:56dc8c2e8cf9 301 /*
bbbobbbieo 37:56dc8c2e8cf9 302 if (center_now >15)
bbbobbbieo 37:56dc8c2e8cf9 303 right_counter =.4;
bbbobbbieo 37:56dc8c2e8cf9 304 else
bbbobbbieo 37:56dc8c2e8cf9 305 {
bbbobbbieo 37:56dc8c2e8cf9 306 */
bbbobbbieo 37:56dc8c2e8cf9 307 if (right_counter <.15)
bbbobbbieo 37:56dc8c2e8cf9 308 right_counter =.15; // less drastic for outside parts
bbbobbbieo 37:56dc8c2e8cf9 309
bbbobbbieo 37:56dc8c2e8cf9 310
bbbobbbieo 37:56dc8c2e8cf9 311 turn_left=false;
bbbobbbieo 37:56dc8c2e8cf9 312 turn_right=true;
bbbobbbieo 37:56dc8c2e8cf9 313
bbbobbbieo 37:56dc8c2e8cf9 314 }
bbbobbbieo 37:56dc8c2e8cf9 315
bbbobbbieo 37:56dc8c2e8cf9 316 else if(num == 3 and left_counter >-.35)
bbbobbbieo 37:56dc8c2e8cf9 317 {
bbbobbbieo 37:56dc8c2e8cf9 318 if(right_counter < 0.4)
bbbobbbieo 37:56dc8c2e8cf9 319 right_counter = 0.4;
bbbobbbieo 37:56dc8c2e8cf9 320
bbbobbbieo 37:56dc8c2e8cf9 321 turn_left = false;
bbbobbbieo 37:56dc8c2e8cf9 322 turn_right = true;
bbbobbbieo 37:56dc8c2e8cf9 323 }
bbbobbbieo 37:56dc8c2e8cf9 324
bbbobbbieo 37:56dc8c2e8cf9 325 else if (num==2 and left_counter >-.45)
bbbobbbieo 37:56dc8c2e8cf9 326 {
bbbobbbieo 37:56dc8c2e8cf9 327 right_counter =.6;
bbbobbbieo 37:56dc8c2e8cf9 328 //right_counter =.55;
bbbobbbieo 37:56dc8c2e8cf9 329 turn_left=false;
bbbobbbieo 37:56dc8c2e8cf9 330 turn_right=true;
bbbobbbieo 37:56dc8c2e8cf9 331 }
bbbobbbieo 37:56dc8c2e8cf9 332
bbbobbbieo 37:56dc8c2e8cf9 333 //straight
bbbobbbieo 37:56dc8c2e8cf9 334 else if (turn_right == false and turn_left == false)
bbbobbbieo 37:56dc8c2e8cf9 335 {
bbbobbbieo 37:56dc8c2e8cf9 336 //TFC_SetServo(0,(0.0+ bullshit_offset));
bbbobbbieo 37:56dc8c2e8cf9 337 TFC_SetServo(0,bullshit_offset);
bbbobbbieo 37:56dc8c2e8cf9 338 TFC_SetMotorPWM(current_left_motor_speed-(.0008*num_of_straight), current_right_motor_speed+(.0008*num_of_straight)); // --left is faster, ++right is faster
bbbobbbieo 37:56dc8c2e8cf9 339 if (violence_level !=0)
bbbobbbieo 37:56dc8c2e8cf9 340 num_of_straight++;
bbbobbbieo 37:56dc8c2e8cf9 341 }
bbbobbbieo 37:56dc8c2e8cf9 342
bbbobbbieo 37:56dc8c2e8cf9 343 else
bbbobbbieo 37:56dc8c2e8cf9 344 {
bbbobbbieo 37:56dc8c2e8cf9 345 }
bbbobbbieo 37:56dc8c2e8cf9 346
bbbobbbieo 37:56dc8c2e8cf9 347 //dealwiththeshit
bbbobbbieo 37:56dc8c2e8cf9 348 if(turn_left)
bbbobbbieo 37:56dc8c2e8cf9 349 {
bbbobbbieo 37:56dc8c2e8cf9 350 turn_right = false;
bbbobbbieo 37:56dc8c2e8cf9 351 num_of_straight = 0; // no longer on a straight
bbbobbbieo 37:56dc8c2e8cf9 352
bbbobbbieo 37:56dc8c2e8cf9 353 if (violence_level == 2 or violence_level == 1)
bbbobbbieo 37:56dc8c2e8cf9 354 TFC_SetServo(0,left_counter+ bullshit_offset );
bbbobbbieo 37:56dc8c2e8cf9 355 if (violence_level == 3 or violence_level == 4)
bbbobbbieo 37:56dc8c2e8cf9 356 TFC_SetServo(0,(left_counter+ bullshit_offset)*.8 );
bbbobbbieo 37:56dc8c2e8cf9 357
bbbobbbieo 37:56dc8c2e8cf9 358 //TFC_SetServo(0,left_counter);
bbbobbbieo 37:56dc8c2e8cf9 359 left_counter += .01;
bbbobbbieo 37:56dc8c2e8cf9 360 if (left_counter > (0+ bullshit_offset))
bbbobbbieo 37:56dc8c2e8cf9 361 turn_left = false;
bbbobbbieo 37:56dc8c2e8cf9 362
bbbobbbieo 37:56dc8c2e8cf9 363 TFC_SetMotorPWM(current_left_motor_speed+(.2*left_counter), current_right_motor_speed+(.2*left_counter)); // ++left is slower, ++right is faster
bbbobbbieo 37:56dc8c2e8cf9 364
bbbobbbieo 37:56dc8c2e8cf9 365
bbbobbbieo 37:56dc8c2e8cf9 366
bbbobbbieo 37:56dc8c2e8cf9 367 }
bbbobbbieo 37:56dc8c2e8cf9 368
bbbobbbieo 37:56dc8c2e8cf9 369 if(turn_right)
bbbobbbieo 37:56dc8c2e8cf9 370 {
bbbobbbieo 37:56dc8c2e8cf9 371 turn_left =false;
bbbobbbieo 37:56dc8c2e8cf9 372 num_of_straight = 0; // no longer on a straight
bbbobbbieo 37:56dc8c2e8cf9 373
bbbobbbieo 37:56dc8c2e8cf9 374 //TFC_SetServo(0,right_counter- bullshit_offset );
bbbobbbieo 37:56dc8c2e8cf9 375 //TFC_SetServo(0,right_counter);
bbbobbbieo 37:56dc8c2e8cf9 376
bbbobbbieo 37:56dc8c2e8cf9 377 if (violence_level == 2 or violence_level == 1)
bbbobbbieo 37:56dc8c2e8cf9 378 TFC_SetServo(0,(right_counter+ bullshit_offset));
bbbobbbieo 37:56dc8c2e8cf9 379 if (violence_level == 3 or violence_level == 4)
bbbobbbieo 37:56dc8c2e8cf9 380 TFC_SetServo(0,(right_counter+ bullshit_offset)*.8 );
bbbobbbieo 37:56dc8c2e8cf9 381
bbbobbbieo 37:56dc8c2e8cf9 382 right_counter -= .01;
bbbobbbieo 37:56dc8c2e8cf9 383 if (right_counter < (0+ bullshit_offset))
bbbobbbieo 37:56dc8c2e8cf9 384 turn_right = false;
bbbobbbieo 37:56dc8c2e8cf9 385
bbbobbbieo 37:56dc8c2e8cf9 386 TFC_SetMotorPWM(current_left_motor_speed-(.2*right_counter), current_right_motor_speed-(.2*right_counter)); // --left is faster, --right is slower
bbbobbbieo 37:56dc8c2e8cf9 387 }
bbbobbbieo 37:56dc8c2e8cf9 388
bbbobbbieo 37:56dc8c2e8cf9 389 // clearing values for next image processing round
bbbobbbieo 37:56dc8c2e8cf9 390 black_value_count = 0;
bbbobbbieo 37:56dc8c2e8cf9 391 sum_black = 0;
bbbobbbieo 37:56dc8c2e8cf9 392 values_list_add =0;
bbbobbbieo 37:56dc8c2e8cf9 393 value_count =0;
bbbobbbieo 37:56dc8c2e8cf9 394 avg_value=0;
bbbobbbieo 37:56dc8c2e8cf9 395 black_threshhold=0;
bbbobbbieo 37:56dc8c2e8cf9 396
bbbobbbieo 37:56dc8c2e8cf9 397 black_list_one = 0;
bbbobbbieo 37:56dc8c2e8cf9 398 white_list = 0;
bbbobbbieo 37:56dc8c2e8cf9 399 black_list_two = 0;
bbbobbbieo 37:56dc8c2e8cf9 400 whiteflag = false;
bbbobbbieo 37:56dc8c2e8cf9 401 blackflag = false;
bbbobbbieo 37:56dc8c2e8cf9 402 stopflag = false;
bbbobbbieo 37:56dc8c2e8cf9 403
bbbobbbieo 37:56dc8c2e8cf9 404 // end image processing
bbbobbbieo 37:56dc8c2e8cf9 405
bbbobbbieo 37:56dc8c2e8cf9 406 linescan_ping_pong = false;
bbbobbbieo 37:56dc8c2e8cf9 407 } // end checking channel 0
bbbobbbieo 37:56dc8c2e8cf9 408
bbbobbbieo 37:56dc8c2e8cf9 409 else { //checking channel 1
bbbobbbieo 37:56dc8c2e8cf9 410 linescan_ping_pong = true;
bbbobbbieo 37:56dc8c2e8cf9 411 }
bbbobbbieo 37:56dc8c2e8cf9 412
bbbobbbieo 37:56dc8c2e8cf9 413 TFC_LineScanImageReady = 0; // since we used it, we reset the flag
bbbobbbieo 37:56dc8c2e8cf9 414 }// end imageready
bbbobbbieo 37:56dc8c2e8cf9 415 }// end linescan stuff
bbbobbbieo 37:56dc8c2e8cf9 416 }
bbbobbbieo 37:56dc8c2e8cf9 417 }
bbbobbbieo 37:56dc8c2e8cf9 418
bbbobbbieo 37:56dc8c2e8cf9 419 // shit code down here