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_1615h.txt@14:1a408e13679d, 2015-03-26 (annotated)

Committer:
bbbobbbieo
Date:
Thu Mar 26 20:18:04 2015 +0000
Revision:
14:1a408e13679d
garbage and shit, added two cases. mikes working on a linear function with 4 cases, we're going to move to that

Who changed what in which revision?

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