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@23:a141ca857b8f, 2015-03-27 (annotated)

Committer:
bbbobbbieo
Date:
Fri Mar 27 19:17:53 2015 +0000
Revision:
23:a141ca857b8f
Parent:
22:539311b65796
Child:
25:806e67a1218f
no longer running against left wheel. doesnt run full track, but is pretty alright. stops when stop marker. fuck

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 19:545e9ddba0e8 9 const float CONSERVATIVE =.33;
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
bbbobbbieo 0:bf39986ebff7 68 int position = 0;
bbbobbbieo 0:bf39986ebff7 69 int set_point = 63;
bbbobbbieo 0:bf39986ebff7 70 int previous_error = 0;
bbbobbbieo 0:bf39986ebff7 71 int error = 0;
mperella 8:2a238dbd0386 72
mperella 8:2a238dbd0386 73 float increment = 0;
mperella 8:2a238dbd0386 74 int right_turn_count = 0;
mperella 8:2a238dbd0386 75 int left_turn_count = 0;
bbbobbbieo 0:bf39986ebff7 76
bbbobbbieo 0:bf39986ebff7 77 for(int i = 0; i < 50; i++)
bbbobbbieo 0:bf39986ebff7 78 centers_List[i] = 63;
bbbobbbieo 0:bf39986ebff7 79
bbbobbbieo 0:bf39986ebff7 80 float left_counter =0;
bbbobbbieo 0:bf39986ebff7 81 float right_counter =0;
bbbobbbieo 0:bf39986ebff7 82 bool turn_left=false;
bbbobbbieo 0:bf39986ebff7 83 bool turn_right=false;
bbbobbbieo 7:f21986164bf1 84 bool need_decel=false;
bbbobbbieo 9:c4a2a99b61e0 85 int num_of_straight =0;
bbbobbbieo 12:915f22e7d7d9 86 int num_of_left =0;
bbbobbbieo 12:915f22e7d7d9 87 int num_of_right=0;
bbbobbbieo 0:bf39986ebff7 88
bbbobbbieo 6:ed97e4324202 89 //servo is offset zero by some bullshit number
bbbobbbieo 16:25eb20a73e59 90 float bullshit_offset = .068;
bbbobbbieo 0:bf39986ebff7 91
bbbobbbieo 0:bf39986ebff7 92 // major loop
bbbobbbieo 0:bf39986ebff7 93 while(1) {
bbbobbbieo 0:bf39986ebff7 94
bbbobbbieo 0:bf39986ebff7 95 // initial motor stuff
bbbobbbieo 0:bf39986ebff7 96 if(rear_motor_enable_flag) {
bbbobbbieo 0:bf39986ebff7 97 TFC_HBRIDGE_ENABLE;
bbbobbbieo 0:bf39986ebff7 98
bbbobbbieo 0:bf39986ebff7 99
bbbobbbieo 0:bf39986ebff7 100 // checking behavior level
bbbobbbieo 0:bf39986ebff7 101 violence_level = int(TFC_GetDIP_Switch());
bbbobbbieo 0:bf39986ebff7 102
bbbobbbieo 0:bf39986ebff7 103 if (violence_level==3) {
bbbobbbieo 0:bf39986ebff7 104 current_left_motor_speed = -(AGGRESSIVE);
bbbobbbieo 0:bf39986ebff7 105 current_right_motor_speed = AGGRESSIVE;
mperella 8:2a238dbd0386 106 increment = 0.02;
bbbobbbieo 0:bf39986ebff7 107 }
bbbobbbieo 0:bf39986ebff7 108 else if (violence_level==2) {
bbbobbbieo 0:bf39986ebff7 109 current_left_motor_speed = -(MODERATE);
bbbobbbieo 0:bf39986ebff7 110 current_right_motor_speed = (MODERATE);
mperella 8:2a238dbd0386 111 increment = 0.03;
bbbobbbieo 0:bf39986ebff7 112 }
bbbobbbieo 0:bf39986ebff7 113 else if (violence_level==1) {
bbbobbbieo 0:bf39986ebff7 114 current_left_motor_speed = -(CONSERVATIVE);
bbbobbbieo 0:bf39986ebff7 115 current_right_motor_speed = CONSERVATIVE;
mperella 8:2a238dbd0386 116 increment = 0.04;
bbbobbbieo 0:bf39986ebff7 117 }
bbbobbbieo 0:bf39986ebff7 118 else if (violence_level==0) {
bbbobbbieo 0:bf39986ebff7 119 current_left_motor_speed = STOP;
bbbobbbieo 0:bf39986ebff7 120 current_right_motor_speed = STOP;
bbbobbbieo 13:8a667757beb3 121 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
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 13:8a667757beb3 126 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
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 0:bf39986ebff7 139 }// end motor enabled
bbbobbbieo 0:bf39986ebff7 140 else {
bbbobbbieo 0:bf39986ebff7 141 TFC_HBRIDGE_DISABLE;
bbbobbbieo 0:bf39986ebff7 142 }// end motor disabled
bbbobbbieo 0:bf39986ebff7 143
bbbobbbieo 0:bf39986ebff7 144 // camera stuff
bbbobbbieo 0:bf39986ebff7 145 if (linescan_enable) {
bbbobbbieo 0:bf39986ebff7 146 if (TFC_LineScanImageReady !=0) {
bbbobbbieo 0:bf39986ebff7 147
bbbobbbieo 0:bf39986ebff7 148 if (linescan_ping_pong) {
bbbobbbieo 0:bf39986ebff7 149 //checking channel 0
bbbobbbieo 0:bf39986ebff7 150
bbbobbbieo 0:bf39986ebff7 151 //checking center pixel, displays aprox value on leds
bbbobbbieo 0:bf39986ebff7 152 uint8_t shitnum = 1;
bbbobbbieo 0:bf39986ebff7 153
bbbobbbieo 0:bf39986ebff7 154 // checking for center line (single line)
bbbobbbieo 14:1a408e13679d 155 for (uint16_t i=10; i<118; i++) {
bbbobbbieo 22:539311b65796 156 if ((*(TFC_LineScanImage0+i) < 425)) {
bbbobbbieo 0:bf39986ebff7 157 black_values_list[black_value_count] = i;
bbbobbbieo 0:bf39986ebff7 158 black_value_count++;
bbbobbbieo 0:bf39986ebff7 159 }
bbbobbbieo 0:bf39986ebff7 160 }
bbbobbbieo 0:bf39986ebff7 161
bbbobbbieo 22:539311b65796 162 for(int i=0; i<black_value_count; i++)
bbbobbbieo 0:bf39986ebff7 163 sum_black += black_values_list[i];
bbbobbbieo 0:bf39986ebff7 164
bbbobbbieo 0:bf39986ebff7 165 //update history
bbbobbbieo 0:bf39986ebff7 166 center_past_4= center_past_3;
bbbobbbieo 0:bf39986ebff7 167 center_past_3= center_past_2;
bbbobbbieo 0:bf39986ebff7 168 center_past_2= center_past_1;
bbbobbbieo 0:bf39986ebff7 169 center_past_1= center_now;
bbbobbbieo 0:bf39986ebff7 170
bbbobbbieo 0:bf39986ebff7 171 center_now = sum_black / black_value_count;
bbbobbbieo 0:bf39986ebff7 172
bbbobbbieo 0:bf39986ebff7 173 uint8_t num = 0;
bbbobbbieo 0:bf39986ebff7 174
bbbobbbieo 15:60d699b587c5 175 if(center_now > 10 && center_now < 45) // linear func
bbbobbbieo 0:bf39986ebff7 176 num = 1;
bbbobbbieo 16:25eb20a73e59 177 else if(center_now >= 45 && center_now < 65) // snap
bbbobbbieo 0:bf39986ebff7 178 num = 2;
bbbobbbieo 16:25eb20a73e59 179 else if(center_now > 64 && center_now < 64) // nothing zone
bbbobbbieo 0:bf39986ebff7 180 num = 15;
bbbobbbieo 15:60d699b587c5 181 else if(center_now >= 65 && center_now < 83) // snap
bbbobbbieo 0:bf39986ebff7 182 num = 4;
bbbobbbieo 15:60d699b587c5 183 else if(center_now >= 83 && center_now < 118) // linear func
bbbobbbieo 0:bf39986ebff7 184 num = 8;
bbbobbbieo 0:bf39986ebff7 185
bbbobbbieo 0:bf39986ebff7 186 else
bbbobbbieo 0:bf39986ebff7 187 num = 0;
bbbobbbieo 0:bf39986ebff7 188
bbbobbbieo 6:ed97e4324202 189 // get rid of garbage data sets
bbbobbbieo 2:5eb97170c199 190 if (black_value_count<2)
bbbobbbieo 2:5eb97170c199 191 num = 0;
bbbobbbieo 2:5eb97170c199 192
bbbobbbieo 19:545e9ddba0e8 193 if (black_value_count>30)
bbbobbbieo 19:545e9ddba0e8 194 {
bbbobbbieo 19:545e9ddba0e8 195 while(1)
bbbobbbieo 19:545e9ddba0e8 196 TFC_SetMotorPWM(0, 0);
bbbobbbieo 19:545e9ddba0e8 197 }
bbbobbbieo 19:545e9ddba0e8 198
bbbobbbieo 0:bf39986ebff7 199 TFC_SetBatteryLED(num);
bbbobbbieo 0:bf39986ebff7 200
bbbobbbieo 0:bf39986ebff7 201 // best guess of center based on weighted average of history
bbbobbbieo 0:bf39986ebff7 202 black_center_value = center_now;
bbbobbbieo 0:bf39986ebff7 203
bbbobbbieo 0:bf39986ebff7 204
bbbobbbieo 0:bf39986ebff7 205 // turn left
bbbobbbieo 6:ed97e4324202 206 // hit wall a little bit on the right
bbbobbbieo 15:60d699b587c5 207 if (num==8 and right_counter <.2 ) // only if we arent turning right
bbbobbbieo 0:bf39986ebff7 208 {
bbbobbbieo 6:ed97e4324202 209 //turn away a little bit for each frame that is wall
bbbobbbieo 14:1a408e13679d 210 if (left_counter >-.45)
bbbobbbieo 19:545e9ddba0e8 211 {
bbbobbbieo 20:548b20634fb4 212 if (center_now>100)
bbbobbbieo 22:539311b65796 213 //left_counter -=.03;
bbbobbbieo 22:539311b65796 214 left_counter -=.027;
bbbobbbieo 19:545e9ddba0e8 215 else
bbbobbbieo 22:539311b65796 216 //left_counter -=.05;
bbbobbbieo 22:539311b65796 217 left_counter -=.044;
bbbobbbieo 19:545e9ddba0e8 218 }
bbbobbbieo 19:545e9ddba0e8 219
bbbobbbieo 19:545e9ddba0e8 220 if (left_counter <-.46)
bbbobbbieo 19:545e9ddba0e8 221 {
bbbobbbieo 19:545e9ddba0e8 222 num_of_left =0;
bbbobbbieo 19:545e9ddba0e8 223 }
bbbobbbieo 19:545e9ddba0e8 224
bbbobbbieo 14:1a408e13679d 225
bbbobbbieo 14:1a408e13679d 226 turn_left=true;
bbbobbbieo 14:1a408e13679d 227 turn_right=false;
bbbobbbieo 14:1a408e13679d 228 }
bbbobbbieo 14:1a408e13679d 229
bbbobbbieo 6:ed97e4324202 230 // turn left real hard
bbbobbbieo 6:ed97e4324202 231 // wall is close to center on right
bbbobbbieo 12:915f22e7d7d9 232 if (num==4 and right_counter <.2) // only if we arent turning right
bbbobbbieo 0:bf39986ebff7 233 {
bbbobbbieo 14:1a408e13679d 234 left_counter=-0.54;
bbbobbbieo 0:bf39986ebff7 235 turn_left=true;
bbbobbbieo 0:bf39986ebff7 236 turn_right=false;
bbbobbbieo 19:545e9ddba0e8 237
bbbobbbieo 19:545e9ddba0e8 238 num_of_left =0;
bbbobbbieo 7:f21986164bf1 239 need_decel=true;
bbbobbbieo 0:bf39986ebff7 240 }
bbbobbbieo 0:bf39986ebff7 241
bbbobbbieo 14:1a408e13679d 242 // turn right hard
bbbobbbieo 14:1a408e13679d 243 // wall is close to center on left
bbbobbbieo 14:1a408e13679d 244 else if (num==2 and left_counter >-.2)// only if we arent turning left
bbbobbbieo 14:1a408e13679d 245 {
bbbobbbieo 14:1a408e13679d 246 right_counter =.54;
bbbobbbieo 14:1a408e13679d 247 turn_left=false;
bbbobbbieo 14:1a408e13679d 248 turn_right=true;
bbbobbbieo 19:545e9ddba0e8 249
bbbobbbieo 19:545e9ddba0e8 250 num_of_right =0;
bbbobbbieo 14:1a408e13679d 251 need_decel=true;
bbbobbbieo 14:1a408e13679d 252 }
bbbobbbieo 14:1a408e13679d 253
bbbobbbieo 14:1a408e13679d 254
bbbobbbieo 6:ed97e4324202 255 // turn right
bbbobbbieo 6:ed97e4324202 256 // hit wall a little bit on the left
bbbobbbieo 15:60d699b587c5 257 else if (num==1 and left_counter >-.2) // only if we arent turning left
bbbobbbieo 6:ed97e4324202 258 {
bbbobbbieo 6:ed97e4324202 259 //turn away a little bit for each frame that is wall
bbbobbbieo 14:1a408e13679d 260 if (right_counter <.45)
bbbobbbieo 19:545e9ddba0e8 261 {
bbbobbbieo 20:548b20634fb4 262 if (center_now<28)
bbbobbbieo 19:545e9ddba0e8 263 right_counter +=.03;
bbbobbbieo 19:545e9ddba0e8 264 else
bbbobbbieo 19:545e9ddba0e8 265 right_counter +=.05;
bbbobbbieo 19:545e9ddba0e8 266 }
bbbobbbieo 19:545e9ddba0e8 267
bbbobbbieo 19:545e9ddba0e8 268 if (right_counter >.46)
bbbobbbieo 19:545e9ddba0e8 269 {
bbbobbbieo 19:545e9ddba0e8 270 num_of_right =0;
bbbobbbieo 19:545e9ddba0e8 271 }
bbbobbbieo 6:ed97e4324202 272
bbbobbbieo 0:bf39986ebff7 273 turn_left=false;
bbbobbbieo 0:bf39986ebff7 274 turn_right=true;
bbbobbbieo 0:bf39986ebff7 275 }
bbbobbbieo 6:ed97e4324202 276
bbbobbbieo 14:1a408e13679d 277
bbbobbbieo 6:ed97e4324202 278 // going straight yesssss
bbbobbbieo 10:761333231e50 279 else if (turn_right == false and turn_left == false and (violence_level !=0))
bbbobbbieo 2:5eb97170c199 280 {
bbbobbbieo 2:5eb97170c199 281 TFC_SetServo(0,(0.0+ bullshit_offset));
bbbobbbieo 20:548b20634fb4 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 12:915f22e7d7d9 283 num_of_left = 0;
bbbobbbieo 12:915f22e7d7d9 284 num_of_right= 0;
bbbobbbieo 10:761333231e50 285 if (violence_level !=0)
bbbobbbieo 10:761333231e50 286 num_of_straight++;
bbbobbbieo 2:5eb97170c199 287 }
bbbobbbieo 2:5eb97170c199 288
bbbobbbieo 6:ed97e4324202 289 else{}
bbbobbbieo 0:bf39986ebff7 290
bbbobbbieo 0:bf39986ebff7 291 //dealwiththeshit
bbbobbbieo 6:ed97e4324202 292 // set servo and motors according to how much left we need to turn
bbbobbbieo 10:761333231e50 293 if(turn_left and (violence_level !=0))
bbbobbbieo 0:bf39986ebff7 294 {
bbbobbbieo 9:c4a2a99b61e0 295 num_of_straight = 0; // no longer on a straight
bbbobbbieo 12:915f22e7d7d9 296 num_of_right = 0;
bbbobbbieo 12:915f22e7d7d9 297 num_of_left++;
bbbobbbieo 9:c4a2a99b61e0 298 turn_right = false;
bbbobbbieo 20:548b20634fb4 299
bbbobbbieo 23:a141ca857b8f 300 if (left_counter + bullshit_offset > -.30)
bbbobbbieo 16:25eb20a73e59 301 TFC_SetServo(0,left_counter + bullshit_offset ); // set turning servo
bbbobbbieo 20:548b20634fb4 302 else
bbbobbbieo 23:a141ca857b8f 303 TFC_SetServo(0, -.30);
bbbobbbieo 9:c4a2a99b61e0 304
bbbobbbieo 6:ed97e4324202 305 // normalize to center each frame
bbbobbbieo 9:c4a2a99b61e0 306 // left turning is - servo
bbbobbbieo 9:c4a2a99b61e0 307 if(left_counter > -.2) // small turn, normalize quickly
bbbobbbieo 22:539311b65796 308 left_counter += .009;
bbbobbbieo 9:c4a2a99b61e0 309 else // hard turn, normalize slowly
bbbobbbieo 22:539311b65796 310 left_counter += .008;
bbbobbbieo 9:c4a2a99b61e0 311
bbbobbbieo 9:c4a2a99b61e0 312 // no longer turning boolean
bbbobbbieo 2:5eb97170c199 313 if (left_counter > (0+ bullshit_offset))
bbbobbbieo 1:e561f697985b 314 turn_left = false;
bbbobbbieo 6:ed97e4324202 315
bbbobbbieo 9:c4a2a99b61e0 316 if (need_decel) // need to deal with the decel
bbbobbbieo 7:f21986164bf1 317 {
bbbobbbieo 19:545e9ddba0e8 318 TFC_SetMotorPWM(current_left_motor_speed+(.5*left_counter), current_right_motor_speed-(.4*left_counter)); // ++left is slowed,--right is slowed
bbbobbbieo 7:f21986164bf1 319 need_decel = false;
bbbobbbieo 7:f21986164bf1 320 }
bbbobbbieo 9:c4a2a99b61e0 321 else // turning speeds
bbbobbbieo 23:a141ca857b8f 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 9:c4a2a99b61e0 323 }// end of turn left
bbbobbbieo 6:ed97e4324202 324
bbbobbbieo 6:ed97e4324202 325 // set servo and motors according to how much right we need to turn
bbbobbbieo 10:761333231e50 326 if(turn_right and (violence_level !=0))
bbbobbbieo 0:bf39986ebff7 327 {
bbbobbbieo 9:c4a2a99b61e0 328 num_of_straight = 0; // no longer going straight
bbbobbbieo 12:915f22e7d7d9 329 num_of_right++;
bbbobbbieo 12:915f22e7d7d9 330 num_of_left=0;
bbbobbbieo 0:bf39986ebff7 331 turn_left =false;
bbbobbbieo 16:25eb20a73e59 332
bbbobbbieo 16:25eb20a73e59 333 if (right_counter - bullshit_offset < .5)
bbbobbbieo 16:25eb20a73e59 334 TFC_SetServo(0,right_counter - bullshit_offset); // set servo
bbbobbbieo 20:548b20634fb4 335 else
bbbobbbieo 20:548b20634fb4 336 TFC_SetServo(0,.5);
bbbobbbieo 9:c4a2a99b61e0 337
bbbobbbieo 6:ed97e4324202 338 // normalize to center each frame
bbbobbbieo 9:c4a2a99b61e0 339 // right turning is + servo
bbbobbbieo 9:c4a2a99b61e0 340 if(right_counter < .2) // small turn, normalize quickly
bbbobbbieo 12:915f22e7d7d9 341 right_counter -= .017;
bbbobbbieo 9:c4a2a99b61e0 342 else // hard turn, normalize slowly
bbbobbbieo 11:e04dc2090433 343 right_counter -= .01;
bbbobbbieo 9:c4a2a99b61e0 344
bbbobbbieo 9:c4a2a99b61e0 345
bbbobbbieo 9:c4a2a99b61e0 346 // no longer turning boolean
bbbobbbieo 2:5eb97170c199 347 if (right_counter < (0+ bullshit_offset))
bbbobbbieo 1:e561f697985b 348 turn_right = false;
bbbobbbieo 6:ed97e4324202 349
bbbobbbieo 9:c4a2a99b61e0 350 if(need_decel)// need to deal with the decel
bbbobbbieo 7:f21986164bf1 351 {
bbbobbbieo 19:545e9ddba0e8 352 TFC_SetMotorPWM(current_left_motor_speed+(.4*right_counter), current_right_motor_speed-(.5*right_counter)); // ++left is slowed,--right is slowed
bbbobbbieo 7:f21986164bf1 353 need_decel = false;
bbbobbbieo 7:f21986164bf1 354 }
bbbobbbieo 9:c4a2a99b61e0 355 else // turning speeds
bbbobbbieo 20:548b20634fb4 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 9:c4a2a99b61e0 357 } // end with turn right
bbbobbbieo 9:c4a2a99b61e0 358
bbbobbbieo 0:bf39986ebff7 359
bbbobbbieo 0:bf39986ebff7 360 // clearing values for next image processing round
bbbobbbieo 0:bf39986ebff7 361 black_value_count = 0;
bbbobbbieo 0:bf39986ebff7 362 sum_black = 0;
bbbobbbieo 0:bf39986ebff7 363 // end image processing
bbbobbbieo 0:bf39986ebff7 364
bbbobbbieo 0:bf39986ebff7 365 linescan_ping_pong = false;
bbbobbbieo 0:bf39986ebff7 366 } // end checking channel 0
bbbobbbieo 0:bf39986ebff7 367
bbbobbbieo 0:bf39986ebff7 368 else { //checking channel 1
bbbobbbieo 0:bf39986ebff7 369 linescan_ping_pong = true;
bbbobbbieo 0:bf39986ebff7 370 }
bbbobbbieo 0:bf39986ebff7 371
bbbobbbieo 0:bf39986ebff7 372 TFC_LineScanImageReady = 0; // since we used it, we reset the flag
bbbobbbieo 0:bf39986ebff7 373 }// end imageready
bbbobbbieo 0:bf39986ebff7 374 }// end linescan stuff
bbbobbbieo 6:ed97e4324202 375 } // end major loop
bbbobbbieo 6:ed97e4324202 376 }// end main