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_26_2015_1600h.txt@40:14d93245afc4, 2015-04-21 (annotated)

Committer:
bbbobbbieo
Date:
Tue Apr 21 19:09:05 2015 +0000
Revision:
40:14d93245afc4
Parent:
13:8a667757beb3 
double camera bitches

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bbbobbbieo 13:8a667757beb3 1 //#include "mbed.h"
bbbobbbieo 13:8a667757beb3 2 #include "TFC.h"
bbbobbbieo 13:8a667757beb3 3 #include <iostream>
bbbobbbieo 13:8a667757beb3 4 #include <stdio.h>
bbbobbbieo 13:8a667757beb3 5 //#include "serialib.h"
bbbobbbieo 13:8a667757beb3 6
bbbobbbieo 13:8a667757beb3 7 const float AGGRESSIVE = .55;
bbbobbbieo 13:8a667757beb3 8 const float MODERATE =.45;
bbbobbbieo 13:8a667757beb3 9 const float CONSERVATIVE =.39;
bbbobbbieo 13:8a667757beb3 10 const float STOP =0;
bbbobbbieo 13:8a667757beb3 11 const float PROTECTION_THRESHOLD_UPPER =.7;
bbbobbbieo 13:8a667757beb3 12 const float PROTECTION_THRESHOLD_LOWER =-.7;
bbbobbbieo 13:8a667757beb3 13 const float TURN_FORWARD_ACCEL =0.045;
bbbobbbieo 13:8a667757beb3 14 const float TURN_BACKWARD_ACCEL =0.025;
bbbobbbieo 13:8a667757beb3 15 const float SERVO_CAN_MOVE_IN_ONE_FRAME =0.1;
bbbobbbieo 13:8a667757beb3 16 const float SERVO_MAX =.5;
bbbobbbieo 13:8a667757beb3 17
bbbobbbieo 13:8a667757beb3 18 const int BLACK_THRESHOLD =63;
bbbobbbieo 13:8a667757beb3 19 const int LINE_SCAN_LENGTH =128;
bbbobbbieo 13:8a667757beb3 20
bbbobbbieo 13:8a667757beb3 21
bbbobbbieo 13:8a667757beb3 22 DigitalOut myled(LED1);
bbbobbbieo 13:8a667757beb3 23
bbbobbbieo 13:8a667757beb3 24 int main()
bbbobbbieo 13:8a667757beb3 25 {
bbbobbbieo 13:8a667757beb3 26 //run this before anything
bbbobbbieo 13:8a667757beb3 27 TFC_Init();
bbbobbbieo 13:8a667757beb3 28
bbbobbbieo 13:8a667757beb3 29 //variables
bbbobbbieo 13:8a667757beb3 30 float current_servo_position = 0;
bbbobbbieo 13:8a667757beb3 31 float previous_servo_position = 0;
bbbobbbieo 13:8a667757beb3 32 float current_left_motor_speed = 0;
bbbobbbieo 13:8a667757beb3 33 float current_right_motor_speed = 0;
bbbobbbieo 13:8a667757beb3 34
bbbobbbieo 13:8a667757beb3 35 float proportional = 0;
bbbobbbieo 13:8a667757beb3 36 float last_proportional = 0;
bbbobbbieo 13:8a667757beb3 37 float integral = 0;
bbbobbbieo 13:8a667757beb3 38 float derivative = 0;
bbbobbbieo 13:8a667757beb3 39 float output = 0;
bbbobbbieo 13:8a667757beb3 40
bbbobbbieo 13:8a667757beb3 41 // gains on prop, int, der
bbbobbbieo 13:8a667757beb3 42 // subject to change, need to fine tune
bbbobbbieo 13:8a667757beb3 43 float kp = 1.8960;
bbbobbbieo 13:8a667757beb3 44 float ki = 0.6170;
bbbobbbieo 13:8a667757beb3 45 float kd = 1.5590;
bbbobbbieo 13:8a667757beb3 46
bbbobbbieo 13:8a667757beb3 47 bool rear_motor_enable_flag = true;
bbbobbbieo 13:8a667757beb3 48 bool linescan_ping_pong = false;
bbbobbbieo 13:8a667757beb3 49 bool linescan_enable = true;
bbbobbbieo 13:8a667757beb3 50
bbbobbbieo 13:8a667757beb3 51 int black_values_list[LINE_SCAN_LENGTH];
bbbobbbieo 13:8a667757beb3 52 int black_value_count = 0;
bbbobbbieo 13:8a667757beb3 53 int black_center_value = 0;
bbbobbbieo 13:8a667757beb3 54 int sum_black = 0;
bbbobbbieo 13:8a667757beb3 55 int violence_level = 0;
bbbobbbieo 13:8a667757beb3 56
bbbobbbieo 13:8a667757beb3 57 int accelList[3];
bbbobbbieo 13:8a667757beb3 58 int lastAccessed = 0;
bbbobbbieo 13:8a667757beb3 59
bbbobbbieo 13:8a667757beb3 60 int centers_List[50];
bbbobbbieo 13:8a667757beb3 61
bbbobbbieo 13:8a667757beb3 62 int center_now = 63;
bbbobbbieo 13:8a667757beb3 63 int center_past_1 = 63;
bbbobbbieo 13:8a667757beb3 64 int center_past_2 = 63;
bbbobbbieo 13:8a667757beb3 65 int center_past_3 = 63;
bbbobbbieo 13:8a667757beb3 66 int center_past_4 = 63;
bbbobbbieo 13:8a667757beb3 67 //int best_guess_center = 64;
bbbobbbieo 13:8a667757beb3 68
bbbobbbieo 13:8a667757beb3 69 int position = 0;
bbbobbbieo 13:8a667757beb3 70 int set_point = 63;
bbbobbbieo 13:8a667757beb3 71 int previous_error = 0;
bbbobbbieo 13:8a667757beb3 72 int error = 0;
bbbobbbieo 13:8a667757beb3 73
bbbobbbieo 13:8a667757beb3 74 float increment = 0;
bbbobbbieo 13:8a667757beb3 75 int right_turn_count = 0;
bbbobbbieo 13:8a667757beb3 76 int left_turn_count = 0;
bbbobbbieo 13:8a667757beb3 77
bbbobbbieo 13:8a667757beb3 78 for(int i = 0; i < 50; i++)
bbbobbbieo 13:8a667757beb3 79 centers_List[i] = 63;
bbbobbbieo 13:8a667757beb3 80
bbbobbbieo 13:8a667757beb3 81 float left_counter =0;
bbbobbbieo 13:8a667757beb3 82 float right_counter =0;
bbbobbbieo 13:8a667757beb3 83 bool turn_left=false;
bbbobbbieo 13:8a667757beb3 84 bool turn_right=false;
bbbobbbieo 13:8a667757beb3 85 bool need_decel=false;
bbbobbbieo 13:8a667757beb3 86 int num_of_straight =0;
bbbobbbieo 13:8a667757beb3 87 int num_of_left =0;
bbbobbbieo 13:8a667757beb3 88 int num_of_right=0;
bbbobbbieo 13:8a667757beb3 89
bbbobbbieo 13:8a667757beb3 90 //servo is offset zero by some bullshit number
bbbobbbieo 13:8a667757beb3 91 float bullshit_offset = .074;
bbbobbbieo 13:8a667757beb3 92
bbbobbbieo 13:8a667757beb3 93 // major loop
bbbobbbieo 13:8a667757beb3 94 while(1) {
bbbobbbieo 13:8a667757beb3 95
bbbobbbieo 13:8a667757beb3 96 // initial motor stuff
bbbobbbieo 13:8a667757beb3 97 if(rear_motor_enable_flag) {
bbbobbbieo 13:8a667757beb3 98 TFC_HBRIDGE_ENABLE;
bbbobbbieo 13:8a667757beb3 99
bbbobbbieo 13:8a667757beb3 100
bbbobbbieo 13:8a667757beb3 101 // checking behavior level
bbbobbbieo 13:8a667757beb3 102 violence_level = int(TFC_GetDIP_Switch());
bbbobbbieo 13:8a667757beb3 103
bbbobbbieo 13:8a667757beb3 104 if (violence_level==3) {
bbbobbbieo 13:8a667757beb3 105 current_left_motor_speed = -(AGGRESSIVE);
bbbobbbieo 13:8a667757beb3 106 current_right_motor_speed = AGGRESSIVE;
bbbobbbieo 13:8a667757beb3 107 increment = 0.02;
bbbobbbieo 13:8a667757beb3 108 }
bbbobbbieo 13:8a667757beb3 109 else if (violence_level==2) {
bbbobbbieo 13:8a667757beb3 110 current_left_motor_speed = -(MODERATE);
bbbobbbieo 13:8a667757beb3 111 current_right_motor_speed = (MODERATE);
bbbobbbieo 13:8a667757beb3 112 increment = 0.03;
bbbobbbieo 13:8a667757beb3 113 }
bbbobbbieo 13:8a667757beb3 114 else if (violence_level==1) {
bbbobbbieo 13:8a667757beb3 115 current_left_motor_speed = -(CONSERVATIVE);
bbbobbbieo 13:8a667757beb3 116 current_right_motor_speed = CONSERVATIVE;
bbbobbbieo 13:8a667757beb3 117 increment = 0.04;
bbbobbbieo 13:8a667757beb3 118 }
bbbobbbieo 13:8a667757beb3 119 else if (violence_level==0) {
bbbobbbieo 13:8a667757beb3 120 current_left_motor_speed = STOP;
bbbobbbieo 13:8a667757beb3 121 current_right_motor_speed = STOP;
bbbobbbieo 13:8a667757beb3 122 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
bbbobbbieo 13:8a667757beb3 123 }
bbbobbbieo 13:8a667757beb3 124 else {
bbbobbbieo 13:8a667757beb3 125 current_left_motor_speed = STOP;
bbbobbbieo 13:8a667757beb3 126 current_right_motor_speed = STOP;
bbbobbbieo 13:8a667757beb3 127 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
bbbobbbieo 13:8a667757beb3 128 }
bbbobbbieo 13:8a667757beb3 129
bbbobbbieo 13:8a667757beb3 130
bbbobbbieo 13:8a667757beb3 131 // protection block
bbbobbbieo 13:8a667757beb3 132 if(current_left_motor_speed >= PROTECTION_THRESHOLD_UPPER)
bbbobbbieo 13:8a667757beb3 133 current_left_motor_speed= PROTECTION_THRESHOLD_UPPER;
bbbobbbieo 13:8a667757beb3 134 if(current_right_motor_speed >= PROTECTION_THRESHOLD_UPPER)
bbbobbbieo 13:8a667757beb3 135 current_right_motor_speed = PROTECTION_THRESHOLD_UPPER;
bbbobbbieo 13:8a667757beb3 136 if(current_left_motor_speed <= PROTECTION_THRESHOLD_LOWER)
bbbobbbieo 13:8a667757beb3 137 current_left_motor_speed = PROTECTION_THRESHOLD_LOWER;
bbbobbbieo 13:8a667757beb3 138 if(current_right_motor_speed <= PROTECTION_THRESHOLD_LOWER)
bbbobbbieo 13:8a667757beb3 139 current_right_motor_speed = PROTECTION_THRESHOLD_LOWER;
bbbobbbieo 13:8a667757beb3 140
bbbobbbieo 13:8a667757beb3 141 //TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
bbbobbbieo 13:8a667757beb3 142 }// end motor enabled
bbbobbbieo 13:8a667757beb3 143 else {
bbbobbbieo 13:8a667757beb3 144 TFC_HBRIDGE_DISABLE;
bbbobbbieo 13:8a667757beb3 145 }// end motor disabled
bbbobbbieo 13:8a667757beb3 146
bbbobbbieo 13:8a667757beb3 147 // camera stuff
bbbobbbieo 13:8a667757beb3 148 if (linescan_enable) {
bbbobbbieo 13:8a667757beb3 149 if (TFC_LineScanImageReady !=0) {
bbbobbbieo 13:8a667757beb3 150
bbbobbbieo 13:8a667757beb3 151 if (linescan_ping_pong) {
bbbobbbieo 13:8a667757beb3 152 //checking channel 0
bbbobbbieo 13:8a667757beb3 153
bbbobbbieo 13:8a667757beb3 154 //checking center pixel, displays aprox value on leds
bbbobbbieo 13:8a667757beb3 155 uint8_t shitnum = 1;
bbbobbbieo 13:8a667757beb3 156
bbbobbbieo 13:8a667757beb3 157
bbbobbbieo 13:8a667757beb3 158 // checking for center line (single line)
bbbobbbieo 13:8a667757beb3 159 for (uint16_t i=15; i<113; i++) {
bbbobbbieo 13:8a667757beb3 160 if ((*(TFC_LineScanImage0+i) < 450)) {
bbbobbbieo 13:8a667757beb3 161 black_values_list[black_value_count] = i;
bbbobbbieo 13:8a667757beb3 162 black_value_count++;
bbbobbbieo 13:8a667757beb3 163 }
bbbobbbieo 13:8a667757beb3 164 }
bbbobbbieo 13:8a667757beb3 165
bbbobbbieo 13:8a667757beb3 166 for(int i=0; i<black_value_count; i++) {
bbbobbbieo 13:8a667757beb3 167 sum_black += black_values_list[i];
bbbobbbieo 13:8a667757beb3 168 }
bbbobbbieo 13:8a667757beb3 169
bbbobbbieo 13:8a667757beb3 170 //update history
bbbobbbieo 13:8a667757beb3 171 center_past_4= center_past_3;
bbbobbbieo 13:8a667757beb3 172 center_past_3= center_past_2;
bbbobbbieo 13:8a667757beb3 173 center_past_2= center_past_1;
bbbobbbieo 13:8a667757beb3 174 center_past_1= center_now;
bbbobbbieo 13:8a667757beb3 175
bbbobbbieo 13:8a667757beb3 176
bbbobbbieo 13:8a667757beb3 177 //if (black_value_count>2)
bbbobbbieo 13:8a667757beb3 178 center_now = sum_black / black_value_count;
bbbobbbieo 13:8a667757beb3 179
bbbobbbieo 13:8a667757beb3 180 uint8_t num = 0;
bbbobbbieo 13:8a667757beb3 181
bbbobbbieo 13:8a667757beb3 182 if(center_now > 15 && center_now < 27)
bbbobbbieo 13:8a667757beb3 183 num = 1;
bbbobbbieo 13:8a667757beb3 184 else if(center_now >= 27 && center_now < 54)
bbbobbbieo 13:8a667757beb3 185 num = 2;
bbbobbbieo 13:8a667757beb3 186 else if(center_now > 60 && center_now < 70)
bbbobbbieo 13:8a667757beb3 187 num = 15;
bbbobbbieo 13:8a667757beb3 188 else if(center_now >= 54 && center_now < 81)
bbbobbbieo 13:8a667757beb3 189 num = 4;
bbbobbbieo 13:8a667757beb3 190 else if(center_now >= 81 && center_now < 113)
bbbobbbieo 13:8a667757beb3 191 num = 8;
bbbobbbieo 13:8a667757beb3 192
bbbobbbieo 13:8a667757beb3 193 else
bbbobbbieo 13:8a667757beb3 194 num = 0;
bbbobbbieo 13:8a667757beb3 195
bbbobbbieo 13:8a667757beb3 196 // get rid of garbage data sets
bbbobbbieo 13:8a667757beb3 197 if (black_value_count<2)
bbbobbbieo 13:8a667757beb3 198 num = 0;
bbbobbbieo 13:8a667757beb3 199
bbbobbbieo 13:8a667757beb3 200 TFC_SetBatteryLED(num);
bbbobbbieo 13:8a667757beb3 201
bbbobbbieo 13:8a667757beb3 202 // best guess of center based on weighted average of history
bbbobbbieo 13:8a667757beb3 203 black_center_value = center_now;
bbbobbbieo 13:8a667757beb3 204
bbbobbbieo 13:8a667757beb3 205
bbbobbbieo 13:8a667757beb3 206 // turn left
bbbobbbieo 13:8a667757beb3 207 // hit wall a little bit on the right
bbbobbbieo 13:8a667757beb3 208 if (num==8 and right_counter <.2 ) // only if we arent turning right
bbbobbbieo 13:8a667757beb3 209 {
bbbobbbieo 13:8a667757beb3 210 //turn away a little bit for each frame that is wall
bbbobbbieo 13:8a667757beb3 211 if (left_counter >-.4)
bbbobbbieo 13:8a667757beb3 212 left_counter -=.05;
bbbobbbieo 13:8a667757beb3 213 //left_counter -=.03;
bbbobbbieo 13:8a667757beb3 214
bbbobbbieo 13:8a667757beb3 215 turn_left=true;
bbbobbbieo 13:8a667757beb3 216 turn_right=false;
bbbobbbieo 13:8a667757beb3 217 }
bbbobbbieo 13:8a667757beb3 218
bbbobbbieo 13:8a667757beb3 219 // turn left real hard
bbbobbbieo 13:8a667757beb3 220 // wall is close to center on right
bbbobbbieo 13:8a667757beb3 221 if (num==4 and right_counter <.2) // only if we arent turning right
bbbobbbieo 13:8a667757beb3 222 {
bbbobbbieo 13:8a667757beb3 223 left_counter=-0.5;
bbbobbbieo 13:8a667757beb3 224 turn_left=true;
bbbobbbieo 13:8a667757beb3 225 turn_right=false;
bbbobbbieo 13:8a667757beb3 226 need_decel=true;
bbbobbbieo 13:8a667757beb3 227 }
bbbobbbieo 13:8a667757beb3 228
bbbobbbieo 13:8a667757beb3 229 // turn right
bbbobbbieo 13:8a667757beb3 230 // hit wall a little bit on the left
bbbobbbieo 13:8a667757beb3 231 else if (num==1 and left_counter >-.2) // only if we arent turning left
bbbobbbieo 13:8a667757beb3 232 {
bbbobbbieo 13:8a667757beb3 233 //turn away a little bit for each frame that is wall
bbbobbbieo 13:8a667757beb3 234 if (right_counter <.4)
bbbobbbieo 13:8a667757beb3 235 right_counter +=.05;
bbbobbbieo 13:8a667757beb3 236 //right_counter +=.03;
bbbobbbieo 13:8a667757beb3 237
bbbobbbieo 13:8a667757beb3 238 turn_left=false;
bbbobbbieo 13:8a667757beb3 239 turn_right=true;
bbbobbbieo 13:8a667757beb3 240 }
bbbobbbieo 13:8a667757beb3 241
bbbobbbieo 13:8a667757beb3 242 // turn right hard
bbbobbbieo 13:8a667757beb3 243 // wall is close to center on left
bbbobbbieo 13:8a667757beb3 244 else if (num==2 and left_counter >-.2)// only if we arent turning left
bbbobbbieo 13:8a667757beb3 245 {
bbbobbbieo 13:8a667757beb3 246 right_counter =.5;
bbbobbbieo 13:8a667757beb3 247 turn_left=false;
bbbobbbieo 13:8a667757beb3 248 turn_right=true;
bbbobbbieo 13:8a667757beb3 249 need_decel=true;
bbbobbbieo 13:8a667757beb3 250 }
bbbobbbieo 13:8a667757beb3 251
bbbobbbieo 13:8a667757beb3 252 // going straight yesssss
bbbobbbieo 13:8a667757beb3 253 else if (turn_right == false and turn_left == false and (violence_level !=0))
bbbobbbieo 13:8a667757beb3 254 {
bbbobbbieo 13:8a667757beb3 255 TFC_SetServo(0,(0.0+ bullshit_offset));
bbbobbbieo 13:8a667757beb3 256 TFC_SetMotorPWM(current_left_motor_speed-(.00005*num_of_straight), current_right_motor_speed+(.00005*num_of_straight)); // --left is faster, ++right is faster
bbbobbbieo 13:8a667757beb3 257 num_of_left = 0;
bbbobbbieo 13:8a667757beb3 258 num_of_right= 0;
bbbobbbieo 13:8a667757beb3 259 if (violence_level !=0)
bbbobbbieo 13:8a667757beb3 260 num_of_straight++;
bbbobbbieo 13:8a667757beb3 261 }
bbbobbbieo 13:8a667757beb3 262
bbbobbbieo 13:8a667757beb3 263 else{}
bbbobbbieo 13:8a667757beb3 264
bbbobbbieo 13:8a667757beb3 265 //dealwiththeshit
bbbobbbieo 13:8a667757beb3 266 // set servo and motors according to how much left we need to turn
bbbobbbieo 13:8a667757beb3 267 if(turn_left and (violence_level !=0))
bbbobbbieo 13:8a667757beb3 268 {
bbbobbbieo 13:8a667757beb3 269 num_of_straight = 0; // no longer on a straight
bbbobbbieo 13:8a667757beb3 270 num_of_right = 0;
bbbobbbieo 13:8a667757beb3 271 num_of_left++;
bbbobbbieo 13:8a667757beb3 272 turn_right = false;
bbbobbbieo 13:8a667757beb3 273 TFC_SetServo(0,left_counter + bullshit_offset ); // set turning servo
bbbobbbieo 13:8a667757beb3 274
bbbobbbieo 13:8a667757beb3 275 // normalize to center each frame
bbbobbbieo 13:8a667757beb3 276 // left turning is - servo
bbbobbbieo 13:8a667757beb3 277 if(left_counter > -.2) // small turn, normalize quickly
bbbobbbieo 13:8a667757beb3 278 left_counter += .017;
bbbobbbieo 13:8a667757beb3 279 else // hard turn, normalize slowly
bbbobbbieo 13:8a667757beb3 280 left_counter += .01;
bbbobbbieo 13:8a667757beb3 281
bbbobbbieo 13:8a667757beb3 282 // no longer turning boolean
bbbobbbieo 13:8a667757beb3 283 if (left_counter > (0+ bullshit_offset))
bbbobbbieo 13:8a667757beb3 284 turn_left = false;
bbbobbbieo 13:8a667757beb3 285
bbbobbbieo 13:8a667757beb3 286 if (need_decel) // need to deal with the decel
bbbobbbieo 13:8a667757beb3 287 {
bbbobbbieo 13:8a667757beb3 288 TFC_SetMotorPWM(current_left_motor_speed+(.55*left_counter), current_right_motor_speed-(.4*left_counter)); // ++left is slowed,--right is slowed
bbbobbbieo 13:8a667757beb3 289 need_decel = false;
bbbobbbieo 13:8a667757beb3 290 }
bbbobbbieo 13:8a667757beb3 291 else // turning speeds
bbbobbbieo 13:8a667757beb3 292 TFC_SetMotorPWM(current_left_motor_speed+(.3*left_counter), current_right_motor_speed+(.3*left_counter)+(.00005*num_of_left)); // ++left is slowed, ++right is faster
bbbobbbieo 13:8a667757beb3 293 }// end of turn left
bbbobbbieo 13:8a667757beb3 294
bbbobbbieo 13:8a667757beb3 295 // set servo and motors according to how much right we need to turn
bbbobbbieo 13:8a667757beb3 296 if(turn_right and (violence_level !=0))
bbbobbbieo 13:8a667757beb3 297 {
bbbobbbieo 13:8a667757beb3 298 num_of_straight = 0; // no longer going straight
bbbobbbieo 13:8a667757beb3 299 num_of_right++;
bbbobbbieo 13:8a667757beb3 300 num_of_left=0;
bbbobbbieo 13:8a667757beb3 301
bbbobbbieo 13:8a667757beb3 302 turn_left =false;
bbbobbbieo 13:8a667757beb3 303 TFC_SetServo(0,right_counter - bullshit_offset); // set servo
bbbobbbieo 13:8a667757beb3 304
bbbobbbieo 13:8a667757beb3 305 // normalize to center each frame
bbbobbbieo 13:8a667757beb3 306 // right turning is + servo
bbbobbbieo 13:8a667757beb3 307 if(right_counter < .2) // small turn, normalize quickly
bbbobbbieo 13:8a667757beb3 308 right_counter -= .017;
bbbobbbieo 13:8a667757beb3 309 else // hard turn, normalize slowly
bbbobbbieo 13:8a667757beb3 310 right_counter -= .01;
bbbobbbieo 13:8a667757beb3 311
bbbobbbieo 13:8a667757beb3 312
bbbobbbieo 13:8a667757beb3 313 // no longer turning boolean
bbbobbbieo 13:8a667757beb3 314 if (right_counter < (0+ bullshit_offset))
bbbobbbieo 13:8a667757beb3 315 turn_right = false;
bbbobbbieo 13:8a667757beb3 316
bbbobbbieo 13:8a667757beb3 317 if(need_decel)// need to deal with the decel
bbbobbbieo 13:8a667757beb3 318 {
bbbobbbieo 13:8a667757beb3 319 TFC_SetMotorPWM(current_left_motor_speed+(.55*right_counter), current_right_motor_speed-(.4*right_counter)); // ++left is slowed,--right is slowed
bbbobbbieo 13:8a667757beb3 320 need_decel = false;
bbbobbbieo 13:8a667757beb3 321 }
bbbobbbieo 13:8a667757beb3 322 else // turning speeds
bbbobbbieo 13:8a667757beb3 323 TFC_SetMotorPWM(current_left_motor_speed-(.3*right_counter)-(.00005*num_of_right), current_right_motor_speed-(.3*right_counter)); // --left is faster, --right is slowed
bbbobbbieo 13:8a667757beb3 324 } // end with turn right
bbbobbbieo 13:8a667757beb3 325
bbbobbbieo 13:8a667757beb3 326
bbbobbbieo 13:8a667757beb3 327 // clearing values for next image processing round
bbbobbbieo 13:8a667757beb3 328 black_value_count = 0;
bbbobbbieo 13:8a667757beb3 329 sum_black = 0;
bbbobbbieo 13:8a667757beb3 330 // end image processing
bbbobbbieo 13:8a667757beb3 331
bbbobbbieo 13:8a667757beb3 332 linescan_ping_pong = false;
bbbobbbieo 13:8a667757beb3 333 } // end checking channel 0
bbbobbbieo 13:8a667757beb3 334
bbbobbbieo 13:8a667757beb3 335 else { //checking channel 1
bbbobbbieo 13:8a667757beb3 336 linescan_ping_pong = true;
bbbobbbieo 13:8a667757beb3 337 }
bbbobbbieo 13:8a667757beb3 338
bbbobbbieo 13:8a667757beb3 339 TFC_LineScanImageReady = 0; // since we used it, we reset the flag
bbbobbbieo 13:8a667757beb3 340 }// end imageready
bbbobbbieo 13:8a667757beb3 341 }// end linescan stuff
bbbobbbieo 13:8a667757beb3 342 } // end major loop
bbbobbbieo 13:8a667757beb3 343 }// end main