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_27_2015_1500h.txt@24:6970da537a36, 2015-03-27 (annotated)

Committer:
bbbobbbieo
Date:
Fri Mar 27 19:18:29 2015 +0000
Revision:
24:6970da537a36
archived

Who changed what in which revision?

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