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

shit.txt@5:e6c21518fa48, 2015-03-26 (annotated)

Committer:
bbbobbbieo
Date:
Thu Mar 26 02:21:33 2015 +0000
Revision:
5:e6c21518fa48
added text files for logging? im about to cut some shit out;

Who changed what in which revision?

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