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_1400h.txt@20:548b20634fb4, 2015-03-27 (annotated)

Committer:
bbbobbbieo
Date:
Fri Mar 27 18:27:52 2015 +0000
Revision:
20:548b20634fb4
fixed some turning bugs I think. runs real slow :/ dunno if it gets around yet.

Who changed what in which revision?

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