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_1730h.txt@18:5fcec2fb73e5, 2015-03-26 (annotated)

Committer:
bbbobbbieo
Date:
Thu Mar 26 21:37:13 2015 +0000
Revision:
18:5fcec2fb73e5
archive bullshit

Who changed what in which revision?

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