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.
asof3_29_2015_1400h.txt@31:55d26f4ef5f0, 2015-03-29 (annotated)
- Committer:
- bbbobbbieo
- Date:
- Sun Mar 29 18:00:19 2015 +0000
- Revision:
- 31:55d26f4ef5f0
asof3_29_2015_1400h.txt can run the track, has issues stopping, doesnt see the line. using a pot for black levels
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
bbbobbbieo | 31:55d26f4ef5f0 | 1 | //#include "mbed.h" |
bbbobbbieo | 31:55d26f4ef5f0 | 2 | #include "TFC.h" |
bbbobbbieo | 31:55d26f4ef5f0 | 3 | #include <iostream> |
bbbobbbieo | 31:55d26f4ef5f0 | 4 | #include <stdio.h> |
bbbobbbieo | 31:55d26f4ef5f0 | 5 | //#include "serialib.h" |
bbbobbbieo | 31:55d26f4ef5f0 | 6 | |
bbbobbbieo | 31:55d26f4ef5f0 | 7 | const float AGGRESSIVE = .43; |
bbbobbbieo | 31:55d26f4ef5f0 | 8 | const float MODERATE =.41; |
bbbobbbieo | 31:55d26f4ef5f0 | 9 | const float CONSERVATIVE =.39; |
bbbobbbieo | 31:55d26f4ef5f0 | 10 | const float STOP =0; |
bbbobbbieo | 31:55d26f4ef5f0 | 11 | const float PROTECTION_THRESHOLD_UPPER =.7; |
bbbobbbieo | 31:55d26f4ef5f0 | 12 | const float PROTECTION_THRESHOLD_LOWER =-.7; |
bbbobbbieo | 31:55d26f4ef5f0 | 13 | const float TURN_FORWARD_ACCEL =0.045; |
bbbobbbieo | 31:55d26f4ef5f0 | 14 | const float TURN_BACKWARD_ACCEL =0.025; |
bbbobbbieo | 31:55d26f4ef5f0 | 15 | const float SERVO_CAN_MOVE_IN_ONE_FRAME =0.1; |
bbbobbbieo | 31:55d26f4ef5f0 | 16 | const float SERVO_MAX =.5; |
bbbobbbieo | 31:55d26f4ef5f0 | 17 | |
bbbobbbieo | 31:55d26f4ef5f0 | 18 | const int BLACK_THRESHOLD =63; |
bbbobbbieo | 31:55d26f4ef5f0 | 19 | const int LINE_SCAN_LENGTH =128; |
bbbobbbieo | 31:55d26f4ef5f0 | 20 | |
bbbobbbieo | 31:55d26f4ef5f0 | 21 | |
bbbobbbieo | 31:55d26f4ef5f0 | 22 | DigitalOut myled(LED1); |
bbbobbbieo | 31:55d26f4ef5f0 | 23 | |
bbbobbbieo | 31:55d26f4ef5f0 | 24 | int main() |
bbbobbbieo | 31:55d26f4ef5f0 | 25 | { |
bbbobbbieo | 31:55d26f4ef5f0 | 26 | //run this before anything |
bbbobbbieo | 31:55d26f4ef5f0 | 27 | TFC_Init(); |
bbbobbbieo | 31:55d26f4ef5f0 | 28 | |
bbbobbbieo | 31:55d26f4ef5f0 | 29 | //variables |
bbbobbbieo | 31:55d26f4ef5f0 | 30 | float current_servo_position = 0; |
bbbobbbieo | 31:55d26f4ef5f0 | 31 | float current_left_motor_speed = 0; |
bbbobbbieo | 31:55d26f4ef5f0 | 32 | float current_right_motor_speed = 0; |
bbbobbbieo | 31:55d26f4ef5f0 | 33 | |
bbbobbbieo | 31:55d26f4ef5f0 | 34 | // gains on prop, int, der |
bbbobbbieo | 31:55d26f4ef5f0 | 35 | // subject to change, need to fine tune |
bbbobbbieo | 31:55d26f4ef5f0 | 36 | |
bbbobbbieo | 31:55d26f4ef5f0 | 37 | bool rear_motor_enable_flag = true; |
bbbobbbieo | 31:55d26f4ef5f0 | 38 | bool linescan_ping_pong = false; |
bbbobbbieo | 31:55d26f4ef5f0 | 39 | bool linescan_enable = true; |
bbbobbbieo | 31:55d26f4ef5f0 | 40 | |
bbbobbbieo | 31:55d26f4ef5f0 | 41 | int black_values_list[LINE_SCAN_LENGTH]; |
bbbobbbieo | 31:55d26f4ef5f0 | 42 | int black_value_count = 0; |
bbbobbbieo | 31:55d26f4ef5f0 | 43 | int black_center_value = 0; |
bbbobbbieo | 31:55d26f4ef5f0 | 44 | int sum_black = 0; |
bbbobbbieo | 31:55d26f4ef5f0 | 45 | int violence_level = 0; |
bbbobbbieo | 31:55d26f4ef5f0 | 46 | |
bbbobbbieo | 31:55d26f4ef5f0 | 47 | int center_now = 63; |
bbbobbbieo | 31:55d26f4ef5f0 | 48 | int center_past_1 = 63; |
bbbobbbieo | 31:55d26f4ef5f0 | 49 | int center_past_2 = 63; |
bbbobbbieo | 31:55d26f4ef5f0 | 50 | int center_past_3 = 63; |
bbbobbbieo | 31:55d26f4ef5f0 | 51 | int center_past_4 = 63; |
bbbobbbieo | 31:55d26f4ef5f0 | 52 | |
bbbobbbieo | 31:55d26f4ef5f0 | 53 | float left_counter =0; |
bbbobbbieo | 31:55d26f4ef5f0 | 54 | float right_counter =0; |
bbbobbbieo | 31:55d26f4ef5f0 | 55 | bool turn_left=false; |
bbbobbbieo | 31:55d26f4ef5f0 | 56 | bool turn_right=false; |
bbbobbbieo | 31:55d26f4ef5f0 | 57 | |
bbbobbbieo | 31:55d26f4ef5f0 | 58 | float bullshit_offset = .088; |
bbbobbbieo | 31:55d26f4ef5f0 | 59 | |
bbbobbbieo | 31:55d26f4ef5f0 | 60 | int num_of_straight =0; |
bbbobbbieo | 31:55d26f4ef5f0 | 61 | int num_of_left =0; |
bbbobbbieo | 31:55d26f4ef5f0 | 62 | int num_of_right=0; |
bbbobbbieo | 31:55d26f4ef5f0 | 63 | |
bbbobbbieo | 31:55d26f4ef5f0 | 64 | int values_list_add =0; |
bbbobbbieo | 31:55d26f4ef5f0 | 65 | int value_count =0; |
bbbobbbieo | 31:55d26f4ef5f0 | 66 | int avg_value=0; |
bbbobbbieo | 31:55d26f4ef5f0 | 67 | //int black_threshhold=0; |
bbbobbbieo | 31:55d26f4ef5f0 | 68 | int black_threshhold =450; |
bbbobbbieo | 31:55d26f4ef5f0 | 69 | |
bbbobbbieo | 31:55d26f4ef5f0 | 70 | // major loop |
bbbobbbieo | 31:55d26f4ef5f0 | 71 | while(1) { |
bbbobbbieo | 31:55d26f4ef5f0 | 72 | |
bbbobbbieo | 31:55d26f4ef5f0 | 73 | // initial motor stuff |
bbbobbbieo | 31:55d26f4ef5f0 | 74 | if(rear_motor_enable_flag) { |
bbbobbbieo | 31:55d26f4ef5f0 | 75 | TFC_HBRIDGE_ENABLE; |
bbbobbbieo | 31:55d26f4ef5f0 | 76 | |
bbbobbbieo | 31:55d26f4ef5f0 | 77 | // checking behavior level |
bbbobbbieo | 31:55d26f4ef5f0 | 78 | violence_level = int(TFC_GetDIP_Switch()); |
bbbobbbieo | 31:55d26f4ef5f0 | 79 | |
bbbobbbieo | 31:55d26f4ef5f0 | 80 | if (violence_level==3) { |
bbbobbbieo | 31:55d26f4ef5f0 | 81 | current_left_motor_speed = -(AGGRESSIVE); |
bbbobbbieo | 31:55d26f4ef5f0 | 82 | current_right_motor_speed = AGGRESSIVE; |
bbbobbbieo | 31:55d26f4ef5f0 | 83 | } |
bbbobbbieo | 31:55d26f4ef5f0 | 84 | else if (violence_level==2) { |
bbbobbbieo | 31:55d26f4ef5f0 | 85 | current_left_motor_speed = -(MODERATE); |
bbbobbbieo | 31:55d26f4ef5f0 | 86 | current_right_motor_speed = (MODERATE); |
bbbobbbieo | 31:55d26f4ef5f0 | 87 | } |
bbbobbbieo | 31:55d26f4ef5f0 | 88 | else if (violence_level==1) { |
bbbobbbieo | 31:55d26f4ef5f0 | 89 | current_left_motor_speed = -(CONSERVATIVE); |
bbbobbbieo | 31:55d26f4ef5f0 | 90 | current_right_motor_speed = CONSERVATIVE; |
bbbobbbieo | 31:55d26f4ef5f0 | 91 | } |
bbbobbbieo | 31:55d26f4ef5f0 | 92 | else if (violence_level==0) { |
bbbobbbieo | 31:55d26f4ef5f0 | 93 | current_left_motor_speed = STOP; |
bbbobbbieo | 31:55d26f4ef5f0 | 94 | current_right_motor_speed = STOP; |
bbbobbbieo | 31:55d26f4ef5f0 | 95 | } |
bbbobbbieo | 31:55d26f4ef5f0 | 96 | else { |
bbbobbbieo | 31:55d26f4ef5f0 | 97 | current_left_motor_speed = STOP; |
bbbobbbieo | 31:55d26f4ef5f0 | 98 | current_right_motor_speed = STOP; |
bbbobbbieo | 31:55d26f4ef5f0 | 99 | } |
bbbobbbieo | 31:55d26f4ef5f0 | 100 | |
bbbobbbieo | 31:55d26f4ef5f0 | 101 | // protection block |
bbbobbbieo | 31:55d26f4ef5f0 | 102 | if(current_left_motor_speed >= PROTECTION_THRESHOLD_UPPER) |
bbbobbbieo | 31:55d26f4ef5f0 | 103 | current_left_motor_speed= PROTECTION_THRESHOLD_UPPER; |
bbbobbbieo | 31:55d26f4ef5f0 | 104 | if(current_right_motor_speed >= PROTECTION_THRESHOLD_UPPER) |
bbbobbbieo | 31:55d26f4ef5f0 | 105 | current_right_motor_speed = PROTECTION_THRESHOLD_UPPER; |
bbbobbbieo | 31:55d26f4ef5f0 | 106 | if(current_left_motor_speed <= PROTECTION_THRESHOLD_LOWER) |
bbbobbbieo | 31:55d26f4ef5f0 | 107 | current_left_motor_speed = PROTECTION_THRESHOLD_LOWER; |
bbbobbbieo | 31:55d26f4ef5f0 | 108 | if(current_right_motor_speed <= PROTECTION_THRESHOLD_LOWER) |
bbbobbbieo | 31:55d26f4ef5f0 | 109 | current_right_motor_speed = PROTECTION_THRESHOLD_LOWER; |
bbbobbbieo | 31:55d26f4ef5f0 | 110 | |
bbbobbbieo | 31:55d26f4ef5f0 | 111 | TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed); |
bbbobbbieo | 31:55d26f4ef5f0 | 112 | }// end motor enabled |
bbbobbbieo | 31:55d26f4ef5f0 | 113 | else { |
bbbobbbieo | 31:55d26f4ef5f0 | 114 | TFC_HBRIDGE_DISABLE; |
bbbobbbieo | 31:55d26f4ef5f0 | 115 | }// end motor disabled |
bbbobbbieo | 31:55d26f4ef5f0 | 116 | |
bbbobbbieo | 31:55d26f4ef5f0 | 117 | // camera stuff |
bbbobbbieo | 31:55d26f4ef5f0 | 118 | if (linescan_enable) { |
bbbobbbieo | 31:55d26f4ef5f0 | 119 | if (TFC_LineScanImageReady !=0) { |
bbbobbbieo | 31:55d26f4ef5f0 | 120 | |
bbbobbbieo | 31:55d26f4ef5f0 | 121 | if (linescan_ping_pong) { |
bbbobbbieo | 31:55d26f4ef5f0 | 122 | //checking channel 0 |
bbbobbbieo | 31:55d26f4ef5f0 | 123 | |
bbbobbbieo | 31:55d26f4ef5f0 | 124 | |
bbbobbbieo | 31:55d26f4ef5f0 | 125 | // checking for center line (single line) |
bbbobbbieo | 31:55d26f4ef5f0 | 126 | for (uint16_t i=10; i<118; i++) { |
bbbobbbieo | 31:55d26f4ef5f0 | 127 | |
bbbobbbieo | 31:55d26f4ef5f0 | 128 | int black_threshhold =700*(TFC_ReadPot(0)); |
bbbobbbieo | 31:55d26f4ef5f0 | 129 | if ((*(TFC_LineScanImage0+i) < black_threshhold)) { |
bbbobbbieo | 31:55d26f4ef5f0 | 130 | black_values_list[black_value_count] = i; |
bbbobbbieo | 31:55d26f4ef5f0 | 131 | black_value_count++; |
bbbobbbieo | 31:55d26f4ef5f0 | 132 | } |
bbbobbbieo | 31:55d26f4ef5f0 | 133 | } |
bbbobbbieo | 31:55d26f4ef5f0 | 134 | |
bbbobbbieo | 31:55d26f4ef5f0 | 135 | for(int i=0; i<black_value_count; i++) { |
bbbobbbieo | 31:55d26f4ef5f0 | 136 | sum_black += black_values_list[i]; |
bbbobbbieo | 31:55d26f4ef5f0 | 137 | } |
bbbobbbieo | 31:55d26f4ef5f0 | 138 | |
bbbobbbieo | 31:55d26f4ef5f0 | 139 | //update history |
bbbobbbieo | 31:55d26f4ef5f0 | 140 | /* |
bbbobbbieo | 31:55d26f4ef5f0 | 141 | center_past_4= center_past_3; |
bbbobbbieo | 31:55d26f4ef5f0 | 142 | center_past_3= center_past_2; |
bbbobbbieo | 31:55d26f4ef5f0 | 143 | center_past_2= center_past_1; |
bbbobbbieo | 31:55d26f4ef5f0 | 144 | center_past_1= center_now; |
bbbobbbieo | 31:55d26f4ef5f0 | 145 | */ |
bbbobbbieo | 31:55d26f4ef5f0 | 146 | |
bbbobbbieo | 31:55d26f4ef5f0 | 147 | //if (black_value_count>2) |
bbbobbbieo | 31:55d26f4ef5f0 | 148 | center_now = sum_black / black_value_count; |
bbbobbbieo | 31:55d26f4ef5f0 | 149 | |
bbbobbbieo | 31:55d26f4ef5f0 | 150 | uint8_t num = 0; |
bbbobbbieo | 31:55d26f4ef5f0 | 151 | |
bbbobbbieo | 31:55d26f4ef5f0 | 152 | if(center_now > 10 && center_now < 27) |
bbbobbbieo | 31:55d26f4ef5f0 | 153 | num = 1; |
bbbobbbieo | 31:55d26f4ef5f0 | 154 | else if(center_now >= 27 && center_now < 54) |
bbbobbbieo | 31:55d26f4ef5f0 | 155 | num = 2; |
bbbobbbieo | 31:55d26f4ef5f0 | 156 | else if(center_now > 60 && center_now < 70) |
bbbobbbieo | 31:55d26f4ef5f0 | 157 | num = 15; |
bbbobbbieo | 31:55d26f4ef5f0 | 158 | else if(center_now >= 54 && center_now < 81) |
bbbobbbieo | 31:55d26f4ef5f0 | 159 | num = 4; |
bbbobbbieo | 31:55d26f4ef5f0 | 160 | else if(center_now >= 81 && center_now < 118) |
bbbobbbieo | 31:55d26f4ef5f0 | 161 | num = 8; |
bbbobbbieo | 31:55d26f4ef5f0 | 162 | |
bbbobbbieo | 31:55d26f4ef5f0 | 163 | else |
bbbobbbieo | 31:55d26f4ef5f0 | 164 | num = 0; |
bbbobbbieo | 31:55d26f4ef5f0 | 165 | |
bbbobbbieo | 31:55d26f4ef5f0 | 166 | if (black_value_count<2) |
bbbobbbieo | 31:55d26f4ef5f0 | 167 | num = 0; |
bbbobbbieo | 31:55d26f4ef5f0 | 168 | |
bbbobbbieo | 31:55d26f4ef5f0 | 169 | //if (black_value_count>26 and ((*(TFC_LineScanImage0+64) > 450) or (*(TFC_LineScanImage0+63) > 450)or(*(TFC_LineScanImage0+62) > 450))) |
bbbobbbieo | 31:55d26f4ef5f0 | 170 | if (black_value_count>26) |
bbbobbbieo | 31:55d26f4ef5f0 | 171 | { |
bbbobbbieo | 31:55d26f4ef5f0 | 172 | while(1) |
bbbobbbieo | 31:55d26f4ef5f0 | 173 | TFC_SetMotorPWM(0, 0); |
bbbobbbieo | 31:55d26f4ef5f0 | 174 | } |
bbbobbbieo | 31:55d26f4ef5f0 | 175 | |
bbbobbbieo | 31:55d26f4ef5f0 | 176 | TFC_SetBatteryLED(num); |
bbbobbbieo | 31:55d26f4ef5f0 | 177 | |
bbbobbbieo | 31:55d26f4ef5f0 | 178 | // best guess of center based on weighted average of history |
bbbobbbieo | 31:55d26f4ef5f0 | 179 | black_center_value = center_now; |
bbbobbbieo | 31:55d26f4ef5f0 | 180 | |
bbbobbbieo | 31:55d26f4ef5f0 | 181 | |
bbbobbbieo | 31:55d26f4ef5f0 | 182 | // turn left |
bbbobbbieo | 31:55d26f4ef5f0 | 183 | if (num==8 and right_counter <.35) |
bbbobbbieo | 31:55d26f4ef5f0 | 184 | { |
bbbobbbieo | 31:55d26f4ef5f0 | 185 | |
bbbobbbieo | 31:55d26f4ef5f0 | 186 | if (center_now <113) |
bbbobbbieo | 31:55d26f4ef5f0 | 187 | left_counter=-0.4; |
bbbobbbieo | 31:55d26f4ef5f0 | 188 | else |
bbbobbbieo | 31:55d26f4ef5f0 | 189 | { |
bbbobbbieo | 31:55d26f4ef5f0 | 190 | if (left_counter > -.15) |
bbbobbbieo | 31:55d26f4ef5f0 | 191 | left_counter=-0.15; // less drastic for outside parts |
bbbobbbieo | 31:55d26f4ef5f0 | 192 | } |
bbbobbbieo | 31:55d26f4ef5f0 | 193 | turn_left=true; |
bbbobbbieo | 31:55d26f4ef5f0 | 194 | turn_right=false; |
bbbobbbieo | 31:55d26f4ef5f0 | 195 | |
bbbobbbieo | 31:55d26f4ef5f0 | 196 | } |
bbbobbbieo | 31:55d26f4ef5f0 | 197 | if (num==4 and right_counter <.35) |
bbbobbbieo | 31:55d26f4ef5f0 | 198 | { |
bbbobbbieo | 31:55d26f4ef5f0 | 199 | left_counter=-0.6; |
bbbobbbieo | 31:55d26f4ef5f0 | 200 | //left_counter=-0.55; |
bbbobbbieo | 31:55d26f4ef5f0 | 201 | turn_left=true; |
bbbobbbieo | 31:55d26f4ef5f0 | 202 | turn_right=false; |
bbbobbbieo | 31:55d26f4ef5f0 | 203 | } |
bbbobbbieo | 31:55d26f4ef5f0 | 204 | |
bbbobbbieo | 31:55d26f4ef5f0 | 205 | // need to turn right |
bbbobbbieo | 31:55d26f4ef5f0 | 206 | else if (num==1 and left_counter >-.35) |
bbbobbbieo | 31:55d26f4ef5f0 | 207 | { |
bbbobbbieo | 31:55d26f4ef5f0 | 208 | if (center_now >15) |
bbbobbbieo | 31:55d26f4ef5f0 | 209 | right_counter =.4; |
bbbobbbieo | 31:55d26f4ef5f0 | 210 | else |
bbbobbbieo | 31:55d26f4ef5f0 | 211 | { |
bbbobbbieo | 31:55d26f4ef5f0 | 212 | if (right_counter <.15) |
bbbobbbieo | 31:55d26f4ef5f0 | 213 | right_counter =.15; // less drastic for outside parts |
bbbobbbieo | 31:55d26f4ef5f0 | 214 | } |
bbbobbbieo | 31:55d26f4ef5f0 | 215 | |
bbbobbbieo | 31:55d26f4ef5f0 | 216 | turn_left=false; |
bbbobbbieo | 31:55d26f4ef5f0 | 217 | turn_right=true; |
bbbobbbieo | 31:55d26f4ef5f0 | 218 | |
bbbobbbieo | 31:55d26f4ef5f0 | 219 | } |
bbbobbbieo | 31:55d26f4ef5f0 | 220 | |
bbbobbbieo | 31:55d26f4ef5f0 | 221 | else if (num==2 and left_counter >-.35) |
bbbobbbieo | 31:55d26f4ef5f0 | 222 | { |
bbbobbbieo | 31:55d26f4ef5f0 | 223 | right_counter =.6; |
bbbobbbieo | 31:55d26f4ef5f0 | 224 | //right_counter =.55; |
bbbobbbieo | 31:55d26f4ef5f0 | 225 | turn_left=false; |
bbbobbbieo | 31:55d26f4ef5f0 | 226 | turn_right=true; |
bbbobbbieo | 31:55d26f4ef5f0 | 227 | } |
bbbobbbieo | 31:55d26f4ef5f0 | 228 | |
bbbobbbieo | 31:55d26f4ef5f0 | 229 | //straight |
bbbobbbieo | 31:55d26f4ef5f0 | 230 | else if (turn_right == false and turn_left == false) |
bbbobbbieo | 31:55d26f4ef5f0 | 231 | { |
bbbobbbieo | 31:55d26f4ef5f0 | 232 | TFC_SetServo(0,(0.0+ bullshit_offset)); |
bbbobbbieo | 31:55d26f4ef5f0 | 233 | TFC_SetMotorPWM(current_left_motor_speed-(.0004*num_of_straight), current_right_motor_speed+(.0004*num_of_straight)); // --left is faster, ++right is faster |
bbbobbbieo | 31:55d26f4ef5f0 | 234 | if (violence_level !=0) |
bbbobbbieo | 31:55d26f4ef5f0 | 235 | num_of_straight++; |
bbbobbbieo | 31:55d26f4ef5f0 | 236 | } |
bbbobbbieo | 31:55d26f4ef5f0 | 237 | |
bbbobbbieo | 31:55d26f4ef5f0 | 238 | else |
bbbobbbieo | 31:55d26f4ef5f0 | 239 | { |
bbbobbbieo | 31:55d26f4ef5f0 | 240 | } |
bbbobbbieo | 31:55d26f4ef5f0 | 241 | |
bbbobbbieo | 31:55d26f4ef5f0 | 242 | //dealwiththeshit |
bbbobbbieo | 31:55d26f4ef5f0 | 243 | if(turn_left) |
bbbobbbieo | 31:55d26f4ef5f0 | 244 | { |
bbbobbbieo | 31:55d26f4ef5f0 | 245 | turn_right = false; |
bbbobbbieo | 31:55d26f4ef5f0 | 246 | num_of_straight = 0; // no longer on a straight |
bbbobbbieo | 31:55d26f4ef5f0 | 247 | |
bbbobbbieo | 31:55d26f4ef5f0 | 248 | TFC_SetServo(0,left_counter+ bullshit_offset ); |
bbbobbbieo | 31:55d26f4ef5f0 | 249 | //TFC_SetServo(0,left_counter); |
bbbobbbieo | 31:55d26f4ef5f0 | 250 | left_counter += .01; |
bbbobbbieo | 31:55d26f4ef5f0 | 251 | if (left_counter > (0+ bullshit_offset)) |
bbbobbbieo | 31:55d26f4ef5f0 | 252 | turn_left = false; |
bbbobbbieo | 31:55d26f4ef5f0 | 253 | |
bbbobbbieo | 31:55d26f4ef5f0 | 254 | TFC_SetMotorPWM(current_left_motor_speed+(.2*left_counter), current_right_motor_speed+(.2*left_counter)); |
bbbobbbieo | 31:55d26f4ef5f0 | 255 | } |
bbbobbbieo | 31:55d26f4ef5f0 | 256 | |
bbbobbbieo | 31:55d26f4ef5f0 | 257 | if(turn_right) |
bbbobbbieo | 31:55d26f4ef5f0 | 258 | { |
bbbobbbieo | 31:55d26f4ef5f0 | 259 | turn_left =false; |
bbbobbbieo | 31:55d26f4ef5f0 | 260 | num_of_straight = 0; // no longer on a straight |
bbbobbbieo | 31:55d26f4ef5f0 | 261 | |
bbbobbbieo | 31:55d26f4ef5f0 | 262 | //TFC_SetServo(0,right_counter- bullshit_offset ); |
bbbobbbieo | 31:55d26f4ef5f0 | 263 | TFC_SetServo(0,right_counter); |
bbbobbbieo | 31:55d26f4ef5f0 | 264 | right_counter -= .01; |
bbbobbbieo | 31:55d26f4ef5f0 | 265 | if (right_counter < (0+ bullshit_offset)) |
bbbobbbieo | 31:55d26f4ef5f0 | 266 | turn_right = false; |
bbbobbbieo | 31:55d26f4ef5f0 | 267 | |
bbbobbbieo | 31:55d26f4ef5f0 | 268 | TFC_SetMotorPWM(current_left_motor_speed-(.2*right_counter), current_right_motor_speed-(.2*right_counter)); |
bbbobbbieo | 31:55d26f4ef5f0 | 269 | } |
bbbobbbieo | 31:55d26f4ef5f0 | 270 | |
bbbobbbieo | 31:55d26f4ef5f0 | 271 | // clearing values for next image processing round |
bbbobbbieo | 31:55d26f4ef5f0 | 272 | black_value_count = 0; |
bbbobbbieo | 31:55d26f4ef5f0 | 273 | sum_black = 0; |
bbbobbbieo | 31:55d26f4ef5f0 | 274 | values_list_add =0; |
bbbobbbieo | 31:55d26f4ef5f0 | 275 | value_count =0; |
bbbobbbieo | 31:55d26f4ef5f0 | 276 | avg_value=0; |
bbbobbbieo | 31:55d26f4ef5f0 | 277 | black_threshhold=0; |
bbbobbbieo | 31:55d26f4ef5f0 | 278 | |
bbbobbbieo | 31:55d26f4ef5f0 | 279 | // end image processing |
bbbobbbieo | 31:55d26f4ef5f0 | 280 | |
bbbobbbieo | 31:55d26f4ef5f0 | 281 | linescan_ping_pong = false; |
bbbobbbieo | 31:55d26f4ef5f0 | 282 | } // end checking channel 0 |
bbbobbbieo | 31:55d26f4ef5f0 | 283 | |
bbbobbbieo | 31:55d26f4ef5f0 | 284 | else { //checking channel 1 |
bbbobbbieo | 31:55d26f4ef5f0 | 285 | linescan_ping_pong = true; |
bbbobbbieo | 31:55d26f4ef5f0 | 286 | } |
bbbobbbieo | 31:55d26f4ef5f0 | 287 | |
bbbobbbieo | 31:55d26f4ef5f0 | 288 | TFC_LineScanImageReady = 0; // since we used it, we reset the flag |
bbbobbbieo | 31:55d26f4ef5f0 | 289 | }// end imageready |
bbbobbbieo | 31:55d26f4ef5f0 | 290 | }// end linescan stuff |
bbbobbbieo | 31:55d26f4ef5f0 | 291 | } |
bbbobbbieo | 31:55d26f4ef5f0 | 292 | } |
bbbobbbieo | 31:55d26f4ef5f0 | 293 | |
bbbobbbieo | 31:55d26f4ef5f0 | 294 | // shit code down here |
bbbobbbieo | 31:55d26f4ef5f0 | 295 |