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

main.cpp@12:915f22e7d7d9, 2015-03-26 (annotated)

Committer:
bbbobbbieo
Date:
Thu Mar 26 19:41:04 2015 +0000
Revision:
12:915f22e7d7d9
Parent:
11:e04dc2090433
Child:
13:8a667757beb3
did some bullshit;

Who changed what in which revision?

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