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.
main.cpp@22:f097de115024, 2015-03-02 (annotated)
- Committer:
- bbbobbbieo
- Date:
- Mon Mar 02 19:57:48 2015 +0000
- Revision:
- 22:f097de115024
- Parent:
- 21:df5a530208eb
this shit sucks now
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| bbbobbbieo | 3:c7caa058fc50 | 1 | //#include "mbed.h" |
| bbbobbbieo | 1:21d40d90b2f0 | 2 | #include "TFC.h" |
| bbbobbbieo | 0:d57117b2188d | 3 | |
| bbbobbbieo | 0:d57117b2188d | 4 | DigitalOut myled(LED1); |
| bbbobbbieo | 0:d57117b2188d | 5 | |
| bbbobbbieo | 14:f43b386b8b5d | 6 | int main() |
| bbbobbbieo | 1:21d40d90b2f0 | 7 | { |
| bbbobbbieo | 7:455e7dd338ee | 8 | //run this before anything |
| bbbobbbieo | 2:d8a51492b646 | 9 | TFC_Init(); |
| bbbobbbieo | 14:f43b386b8b5d | 10 | |
| bbbobbbieo | 7:455e7dd338ee | 11 | //variables |
| bbbobbbieo | 4:7584ff0426f1 | 12 | float current_servo_position = 0; |
| bbbobbbieo | 6:44d1079f076c | 13 | float current_left_motor_speed = 0; |
| bbbobbbieo | 6:44d1079f076c | 14 | float current_right_motor_speed = 0; |
| mperella | 17:c643b6b4a96f | 15 | |
| mperella | 17:c643b6b4a96f | 16 | float dt = 0.00001; |
| mperella | 17:c643b6b4a96f | 17 | float integral = 0; |
| mperella | 17:c643b6b4a96f | 18 | float derivative = 0; |
| mperella | 17:c643b6b4a96f | 19 | float output = 0; |
| mperella | 17:c643b6b4a96f | 20 | |
| mperella | 17:c643b6b4a96f | 21 | // gains on prop, int, der |
| mperella | 17:c643b6b4a96f | 22 | // subject to change, need to fine tune |
| mperella | 17:c643b6b4a96f | 23 | float kp = 1.8960; |
| mperella | 17:c643b6b4a96f | 24 | float ki = 0.6170; |
| mperella | 17:c643b6b4a96f | 25 | float kd = 1.5590; |
| mperella | 17:c643b6b4a96f | 26 | |
| bbbobbbieo | 6:44d1079f076c | 27 | bool rear_motor_enable_flag = true; |
| bbbobbbieo | 7:455e7dd338ee | 28 | bool linescan_ping_pong = false; |
| bbbobbbieo | 9:2b028ee421ad | 29 | bool linescan_enable = true; |
| bbbobbbieo | 14:f43b386b8b5d | 30 | |
| bbbobbbieo | 11:d65d6d7fc85e | 31 | int black_values_list[128]; |
| bbbobbbieo | 11:d65d6d7fc85e | 32 | int black_value_count = 0; |
| bbbobbbieo | 11:d65d6d7fc85e | 33 | int black_center_value = 0; |
| bbbobbbieo | 11:d65d6d7fc85e | 34 | int sum_black = 0; |
| bbbobbbieo | 22:f097de115024 | 35 | int black_center_value_past_1 =64; |
| bbbobbbieo | 22:f097de115024 | 36 | int black_center_value_current = 64; |
| bbbobbbieo | 22:f097de115024 | 37 | |
| bbbobbbieo | 14:f43b386b8b5d | 38 | int violence_level = 0; |
| bbbobbbieo | 15:830209e846d5 | 39 | |
| bbbobbbieo | 20:c728b8ffad97 | 40 | int center_now = 64; |
| bbbobbbieo | 20:c728b8ffad97 | 41 | int center_past_1 = 64; |
| bbbobbbieo | 20:c728b8ffad97 | 42 | int center_past_2 = 64; |
| bbbobbbieo | 20:c728b8ffad97 | 43 | int center_past_3 = 64; |
| bbbobbbieo | 20:c728b8ffad97 | 44 | int center_past_4 = 64; |
| bbbobbbieo | 20:c728b8ffad97 | 45 | //int best_guess_center = 64; |
| bbbobbbieo | 20:c728b8ffad97 | 46 | |
| bbbobbbieo | 22:f097de115024 | 47 | |
| mperella | 17:c643b6b4a96f | 48 | int set_point = 64; |
| bbbobbbieo | 19:85eb7991e2ab | 49 | int previous_error = 0; |
| mperella | 17:c643b6b4a96f | 50 | int error = 0; |
| mperella | 17:c643b6b4a96f | 51 | |
| mperella | 17:c643b6b4a96f | 52 | |
| bbbobbbieo | 9:2b028ee421ad | 53 | //uint16_t MyImage0Buffer[2][128]; |
| bbbobbbieo | 9:2b028ee421ad | 54 | //uint16_t MyImage1Buffer[2][128]; |
| bbbobbbieo | 14:f43b386b8b5d | 55 | |
| bbbobbbieo | 7:455e7dd338ee | 56 | // major loop |
| bbbobbbieo | 14:f43b386b8b5d | 57 | while(1) { |
| bbbobbbieo | 14:f43b386b8b5d | 58 | |
| bbbobbbieo | 15:830209e846d5 | 59 | // manual servo control, unused |
| bbbobbbieo | 14:f43b386b8b5d | 60 | if (TFC_ReadPushButton(0) != 0 ) { |
| bbbobbbieo | 14:f43b386b8b5d | 61 | current_servo_position = current_servo_position-.005; |
| bbbobbbieo | 14:f43b386b8b5d | 62 | if(current_servo_position <= -0.4) |
| bbbobbbieo | 14:f43b386b8b5d | 63 | current_servo_position = -0.4; |
| bbbobbbieo | 14:f43b386b8b5d | 64 | TFC_SetServo(0, current_servo_position); |
| bbbobbbieo | 14:f43b386b8b5d | 65 | }// end check button0 |
| bbbobbbieo | 14:f43b386b8b5d | 66 | |
| bbbobbbieo | 15:830209e846d5 | 67 | else {} |
| bbbobbbieo | 14:f43b386b8b5d | 68 | |
| bbbobbbieo | 15:830209e846d5 | 69 | // manual servo control, unused |
| bbbobbbieo | 14:f43b386b8b5d | 70 | if (TFC_ReadPushButton(1) != 0 ) { |
| bbbobbbieo | 14:f43b386b8b5d | 71 | current_servo_position = current_servo_position+.005; |
| bbbobbbieo | 14:f43b386b8b5d | 72 | if(current_servo_position >= 0.4) |
| bbbobbbieo | 14:f43b386b8b5d | 73 | current_servo_position = 0.4; |
| bbbobbbieo | 14:f43b386b8b5d | 74 | TFC_SetServo(0, current_servo_position); |
| bbbobbbieo | 14:f43b386b8b5d | 75 | }// end check button1 |
| bbbobbbieo | 14:f43b386b8b5d | 76 | |
| bbbobbbieo | 15:830209e846d5 | 77 | else {} |
| bbbobbbieo | 14:f43b386b8b5d | 78 | |
| bbbobbbieo | 15:830209e846d5 | 79 | // initial motor stuff |
| bbbobbbieo | 14:f43b386b8b5d | 80 | if(rear_motor_enable_flag) { |
| bbbobbbieo | 14:f43b386b8b5d | 81 | TFC_HBRIDGE_ENABLE; |
| bbbobbbieo | 14:f43b386b8b5d | 82 | |
| bbbobbbieo | 15:830209e846d5 | 83 | //current_left_motor_speed = (TFC_ReadPot(0)); |
| bbbobbbieo | 15:830209e846d5 | 84 | //current_right_motor_speed = (TFC_ReadPot(1)); |
| bbbobbbieo | 14:f43b386b8b5d | 85 | |
| bbbobbbieo | 15:830209e846d5 | 86 | // checking behavior level |
| bbbobbbieo | 14:f43b386b8b5d | 87 | violence_level = int(TFC_GetDIP_Switch()); |
| bbbobbbieo | 14:f43b386b8b5d | 88 | |
| bbbobbbieo | 15:830209e846d5 | 89 | if (violence_level==2) { |
| bbbobbbieo | 16:11ba5d6f42ba | 90 | current_left_motor_speed = -.48; |
| bbbobbbieo | 16:11ba5d6f42ba | 91 | current_right_motor_speed = .48; |
| bbbobbbieo | 15:830209e846d5 | 92 | } |
| bbbobbbieo | 15:830209e846d5 | 93 | else if (violence_level==1) { |
| bbbobbbieo | 16:11ba5d6f42ba | 94 | current_left_motor_speed = -.35; |
| bbbobbbieo | 16:11ba5d6f42ba | 95 | current_right_motor_speed = .35; |
| bbbobbbieo | 14:f43b386b8b5d | 96 | } |
| bbbobbbieo | 21:df5a530208eb | 97 | else if (violence_level==3) { |
| bbbobbbieo | 21:df5a530208eb | 98 | current_left_motor_speed = -.55; |
| bbbobbbieo | 21:df5a530208eb | 99 | current_right_motor_speed = .55; |
| bbbobbbieo | 21:df5a530208eb | 100 | } |
| bbbobbbieo | 15:830209e846d5 | 101 | else if (violence_level==0) { |
| bbbobbbieo | 15:830209e846d5 | 102 | current_left_motor_speed = 0; |
| bbbobbbieo | 15:830209e846d5 | 103 | current_right_motor_speed = 0; |
| bbbobbbieo | 15:830209e846d5 | 104 | } |
| bbbobbbieo | 15:830209e846d5 | 105 | else { |
| bbbobbbieo | 15:830209e846d5 | 106 | current_left_motor_speed = 0; |
| bbbobbbieo | 15:830209e846d5 | 107 | current_right_motor_speed = 0; |
| bbbobbbieo | 15:830209e846d5 | 108 | } |
| bbbobbbieo | 15:830209e846d5 | 109 | |
| bbbobbbieo | 14:f43b386b8b5d | 110 | |
| bbbobbbieo | 15:830209e846d5 | 111 | // protection block |
| bbbobbbieo | 21:df5a530208eb | 112 | if(current_left_motor_speed >= 0.7) |
| bbbobbbieo | 21:df5a530208eb | 113 | current_left_motor_speed= 0.7; |
| bbbobbbieo | 21:df5a530208eb | 114 | if(current_right_motor_speed >= 0.7) |
| bbbobbbieo | 21:df5a530208eb | 115 | current_right_motor_speed= 0.7; |
| bbbobbbieo | 21:df5a530208eb | 116 | if(current_left_motor_speed <= -0.7) |
| bbbobbbieo | 21:df5a530208eb | 117 | current_left_motor_speed= -0.7; |
| bbbobbbieo | 21:df5a530208eb | 118 | if(current_right_motor_speed <= -0.7) |
| bbbobbbieo | 21:df5a530208eb | 119 | current_right_motor_speed= -0.7; |
| bbbobbbieo | 14:f43b386b8b5d | 120 | |
| bbbobbbieo | 14:f43b386b8b5d | 121 | TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed); |
| bbbobbbieo | 14:f43b386b8b5d | 122 | }// end motor enabled |
| bbbobbbieo | 14:f43b386b8b5d | 123 | else { |
| bbbobbbieo | 14:f43b386b8b5d | 124 | TFC_HBRIDGE_DISABLE; |
| bbbobbbieo | 14:f43b386b8b5d | 125 | }// end motor disabled |
| bbbobbbieo | 14:f43b386b8b5d | 126 | |
| bbbobbbieo | 15:830209e846d5 | 127 | // camera stuff |
| bbbobbbieo | 14:f43b386b8b5d | 128 | if (linescan_enable) { |
| bbbobbbieo | 14:f43b386b8b5d | 129 | if (TFC_LineScanImageReady !=0) { |
| bbbobbbieo | 14:f43b386b8b5d | 130 | |
| bbbobbbieo | 15:830209e846d5 | 131 | if (linescan_ping_pong) { |
| bbbobbbieo | 15:830209e846d5 | 132 | //checking channel 0 |
| bbbobbbieo | 15:830209e846d5 | 133 | |
| bbbobbbieo | 15:830209e846d5 | 134 | //checking center pixel, displays aprox value on leds |
| bbbobbbieo | 14:f43b386b8b5d | 135 | uint8_t shitnum = 1; |
| bbbobbbieo | 14:f43b386b8b5d | 136 | if (*(TFC_LineScanImage0+64) > 800) |
| bbbobbbieo | 14:f43b386b8b5d | 137 | shitnum = 15; |
| bbbobbbieo | 14:f43b386b8b5d | 138 | else if((*(TFC_LineScanImage0+64) > 450)) |
| bbbobbbieo | 14:f43b386b8b5d | 139 | shitnum = 7; |
| bbbobbbieo | 14:f43b386b8b5d | 140 | else if((*(TFC_LineScanImage0+64) > 400)) |
| bbbobbbieo | 14:f43b386b8b5d | 141 | shitnum = 3; |
| bbbobbbieo | 14:f43b386b8b5d | 142 | else |
| bbbobbbieo | 14:f43b386b8b5d | 143 | shitnum = 1; |
| bbbobbbieo | 14:f43b386b8b5d | 144 | TFC_SetBatteryLED(shitnum); |
| bbbobbbieo | 14:f43b386b8b5d | 145 | |
| bbbobbbieo | 15:830209e846d5 | 146 | |
| bbbobbbieo | 15:830209e846d5 | 147 | // checking for center line (single line) |
| bbbobbbieo | 14:f43b386b8b5d | 148 | for (uint16_t i=0; i<128; i++) { |
| bbbobbbieo | 22:f097de115024 | 149 | if ((*(TFC_LineScanImage0+i) < 300)) { |
| bbbobbbieo | 14:f43b386b8b5d | 150 | black_values_list[black_value_count] = i; |
| bbbobbbieo | 14:f43b386b8b5d | 151 | black_value_count++; |
| bbbobbbieo | 14:f43b386b8b5d | 152 | } |
| bbbobbbieo | 14:f43b386b8b5d | 153 | } |
| bbbobbbieo | 14:f43b386b8b5d | 154 | |
| bbbobbbieo | 14:f43b386b8b5d | 155 | for(int i=0; i<black_value_count; i++) { |
| bbbobbbieo | 14:f43b386b8b5d | 156 | sum_black += black_values_list[i]; |
| bbbobbbieo | 14:f43b386b8b5d | 157 | } |
| bbbobbbieo | 14:f43b386b8b5d | 158 | |
| bbbobbbieo | 20:c728b8ffad97 | 159 | //update history |
| bbbobbbieo | 20:c728b8ffad97 | 160 | center_past_4= center_past_3; |
| bbbobbbieo | 20:c728b8ffad97 | 161 | center_past_3= center_past_2; |
| bbbobbbieo | 20:c728b8ffad97 | 162 | center_past_2= center_past_1; |
| bbbobbbieo | 20:c728b8ffad97 | 163 | center_past_1= center_now; |
| bbbobbbieo | 20:c728b8ffad97 | 164 | |
| bbbobbbieo | 15:830209e846d5 | 165 | // value of center of black (single line) |
| bbbobbbieo | 20:c728b8ffad97 | 166 | //black_center_value = sum_black / black_value_count; |
| bbbobbbieo | 20:c728b8ffad97 | 167 | center_now = sum_black / black_value_count; |
| bbbobbbieo | 20:c728b8ffad97 | 168 | |
| bbbobbbieo | 20:c728b8ffad97 | 169 | // best guess of center based on weighted average of history |
| bbbobbbieo | 22:f097de115024 | 170 | black_center_value_past_1 = black_center_value_current ; |
| bbbobbbieo | 22:f097de115024 | 171 | black_center_value_current = (5*center_now + 5*center_past_1 + 20*center_past_2 +30*center_past_3 +40*center_past_4)/100; |
| bbbobbbieo | 22:f097de115024 | 172 | black_center_value = (60*black_center_value_current + 40*black_center_value_past_1)/100; |
| bbbobbbieo | 15:830209e846d5 | 173 | |
| mperella | 17:c643b6b4a96f | 174 | /* ******* PID ALGORITHM ******* |
| mperella | 17:c643b6b4a96f | 175 | |
| mperella | 17:c643b6b4a96f | 176 | error = set_point - black_center_value; |
| mperella | 17:c643b6b4a96f | 177 | integral = integral + error*dt; |
| mperella | 17:c643b6b4a96f | 178 | derivative = (error - previous_error)/dt; |
| mperella | 17:c643b6b4a96f | 179 | output = kp*error + ki*integral + kd*derivative; |
| mperella | 17:c643b6b4a96f | 180 | previous error = error; |
| mperella | 17:c643b6b4a96f | 181 | |
| mperella | 17:c643b6b4a96f | 182 | GOTTA DO SOME TESTS/SIMULATIONS TO CALIBRATE SERVO ADJUSTMENTS |
| mperella | 17:c643b6b4a96f | 183 | |
| mperella | 17:c643b6b4a96f | 184 | |
| mperella | 17:c643b6b4a96f | 185 | ***************************** |
| mperella | 17:c643b6b4a96f | 186 | */ |
| bbbobbbieo | 14:f43b386b8b5d | 187 | |
| bbbobbbieo | 21:df5a530208eb | 188 | /*if (black_center_value == 64) { |
| bbbobbbieo | 21:df5a530208eb | 189 | |
| bbbobbbieo | 21:df5a530208eb | 190 | TFC_SetServo(0, 0.0); |
| bbbobbbieo | 21:df5a530208eb | 191 | if (violence_level !=0){ |
| bbbobbbieo | 21:df5a530208eb | 192 | current_left_motor_speed = current_left_motor_speed + violence_level*.045;// push more forwards |
| bbbobbbieo | 21:df5a530208eb | 193 | current_right_motor_speed = current_right_motor_speed + violence_level*.045;// push more forwards |
| bbbobbbieo | 21:df5a530208eb | 194 | } |
| bbbobbbieo | 21:df5a530208eb | 195 | }*/ |
| bbbobbbieo | 21:df5a530208eb | 196 | |
| bbbobbbieo | 15:830209e846d5 | 197 | // need to turn left |
| bbbobbbieo | 15:830209e846d5 | 198 | if (black_center_value < 64) { |
| bbbobbbieo | 15:830209e846d5 | 199 | |
| bbbobbbieo | 22:f097de115024 | 200 | //current_servo_position= float(.01875*black_center_value-(1.2)); |
| bbbobbbieo | 22:f097de115024 | 201 | //current_servo_position= float(.015625*black_center_value-(1.0)); |
| bbbobbbieo | 22:f097de115024 | 202 | current_servo_position= float(.0234375*black_center_value-(1.5)); |
| bbbobbbieo | 14:f43b386b8b5d | 203 | if(current_servo_position <= -0.4) |
| bbbobbbieo | 14:f43b386b8b5d | 204 | current_servo_position = -0.4; |
| bbbobbbieo | 14:f43b386b8b5d | 205 | TFC_SetServo(0, current_servo_position); |
| bbbobbbieo | 15:830209e846d5 | 206 | |
| bbbobbbieo | 15:830209e846d5 | 207 | |
| bbbobbbieo | 16:11ba5d6f42ba | 208 | //current_left_motor_speed = current_left_motor_speed + float(64-black_center_value)*.0025; |
| bbbobbbieo | 16:11ba5d6f42ba | 209 | //current_right_motor_speed = current_right_motor_speed + float(64-black_center_value)*.0025; |
| bbbobbbieo | 16:11ba5d6f42ba | 210 | if (violence_level !=0){ |
| bbbobbbieo | 21:df5a530208eb | 211 | current_left_motor_speed = current_left_motor_speed + float(float(64-black_center_value)*.035);// kinda reverse this... |
| bbbobbbieo | 21:df5a530208eb | 212 | current_right_motor_speed = current_right_motor_speed + float(float(64-black_center_value)*.025);// push more forwards |
| bbbobbbieo | 16:11ba5d6f42ba | 213 | } |
| bbbobbbieo | 16:11ba5d6f42ba | 214 | |
| bbbobbbieo | 21:df5a530208eb | 215 | /*// protection block |
| bbbobbbieo | 16:11ba5d6f42ba | 216 | if(current_left_motor_speed >= 0.5) |
| bbbobbbieo | 16:11ba5d6f42ba | 217 | current_left_motor_speed= 0.5; |
| bbbobbbieo | 16:11ba5d6f42ba | 218 | if(current_right_motor_speed >= 0.5) |
| bbbobbbieo | 16:11ba5d6f42ba | 219 | current_right_motor_speed= 0.5; |
| bbbobbbieo | 16:11ba5d6f42ba | 220 | if(current_left_motor_speed <= -0.5) |
| bbbobbbieo | 16:11ba5d6f42ba | 221 | current_left_motor_speed= -0.5; |
| bbbobbbieo | 16:11ba5d6f42ba | 222 | if(current_right_motor_speed <= -0.5) |
| bbbobbbieo | 21:df5a530208eb | 223 | current_right_motor_speed= -0.5;*/ |
| bbbobbbieo | 21:df5a530208eb | 224 | |
| bbbobbbieo | 21:df5a530208eb | 225 | // protection block |
| bbbobbbieo | 21:df5a530208eb | 226 | if(current_left_motor_speed >= 0.7) |
| bbbobbbieo | 21:df5a530208eb | 227 | current_left_motor_speed= 0.7; |
| bbbobbbieo | 21:df5a530208eb | 228 | if(current_right_motor_speed >= 0.7) |
| bbbobbbieo | 21:df5a530208eb | 229 | current_right_motor_speed= 0.7; |
| bbbobbbieo | 21:df5a530208eb | 230 | if(current_left_motor_speed <= -0.7) |
| bbbobbbieo | 21:df5a530208eb | 231 | current_left_motor_speed= -0.7; |
| bbbobbbieo | 21:df5a530208eb | 232 | if(current_right_motor_speed <= -0.7) |
| bbbobbbieo | 21:df5a530208eb | 233 | current_right_motor_speed= -0.7; |
| bbbobbbieo | 16:11ba5d6f42ba | 234 | |
| bbbobbbieo | 14:f43b386b8b5d | 235 | TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed); |
| bbbobbbieo | 14:f43b386b8b5d | 236 | |
| bbbobbbieo | 7:455e7dd338ee | 237 | } |
| bbbobbbieo | 14:f43b386b8b5d | 238 | |
| bbbobbbieo | 15:830209e846d5 | 239 | // need to turn right |
| bbbobbbieo | 14:f43b386b8b5d | 240 | if (black_center_value > 64) { |
| bbbobbbieo | 14:f43b386b8b5d | 241 | |
| bbbobbbieo | 22:f097de115024 | 242 | //current_servo_position= float(.01875*black_center_value-(1.2)); |
| bbbobbbieo | 22:f097de115024 | 243 | //current_servo_position= float(.015625*black_center_value-(1.0)); |
| bbbobbbieo | 22:f097de115024 | 244 | current_servo_position= float(.0234375*black_center_value-(1.5)); |
| bbbobbbieo | 14:f43b386b8b5d | 245 | if( current_servo_position >= +0.4) |
| bbbobbbieo | 14:f43b386b8b5d | 246 | current_servo_position = +0.4; |
| bbbobbbieo | 14:f43b386b8b5d | 247 | TFC_SetServo(0, current_servo_position); |
| bbbobbbieo | 15:830209e846d5 | 248 | |
| bbbobbbieo | 16:11ba5d6f42ba | 249 | //current_left_motor_speed = current_left_motor_speed - float(black_center_value-64)*.0025; |
| bbbobbbieo | 16:11ba5d6f42ba | 250 | //current_right_motor_speed = current_right_motor_speed - float(black_center_value-64)*.0025; |
| bbbobbbieo | 16:11ba5d6f42ba | 251 | if (violence_level !=0){ |
| bbbobbbieo | 21:df5a530208eb | 252 | current_left_motor_speed = current_left_motor_speed - float(float(black_center_value-64)*.025);// push more forwards |
| bbbobbbieo | 21:df5a530208eb | 253 | current_right_motor_speed = current_right_motor_speed - float(float(black_center_value-64)*.035);// kinda reverse this... |
| bbbobbbieo | 16:11ba5d6f42ba | 254 | } |
| bbbobbbieo | 15:830209e846d5 | 255 | |
| bbbobbbieo | 21:df5a530208eb | 256 | /*// protection block |
| bbbobbbieo | 16:11ba5d6f42ba | 257 | if(current_left_motor_speed >= 0.5) |
| bbbobbbieo | 16:11ba5d6f42ba | 258 | current_left_motor_speed= 0.5; |
| bbbobbbieo | 16:11ba5d6f42ba | 259 | if(current_right_motor_speed >= 0.5) |
| bbbobbbieo | 16:11ba5d6f42ba | 260 | current_right_motor_speed= 0.5; |
| bbbobbbieo | 16:11ba5d6f42ba | 261 | if(current_left_motor_speed <= -0.5) |
| bbbobbbieo | 16:11ba5d6f42ba | 262 | current_left_motor_speed= -0.5; |
| bbbobbbieo | 16:11ba5d6f42ba | 263 | if(current_right_motor_speed <= -0.5) |
| bbbobbbieo | 21:df5a530208eb | 264 | current_right_motor_speed= -0.5;*/ |
| bbbobbbieo | 21:df5a530208eb | 265 | |
| bbbobbbieo | 21:df5a530208eb | 266 | // protection block |
| bbbobbbieo | 21:df5a530208eb | 267 | if(current_left_motor_speed >= 0.7) |
| bbbobbbieo | 21:df5a530208eb | 268 | current_left_motor_speed= 0.7; |
| bbbobbbieo | 21:df5a530208eb | 269 | if(current_right_motor_speed >= 0.7) |
| bbbobbbieo | 21:df5a530208eb | 270 | current_right_motor_speed= 0.7; |
| bbbobbbieo | 21:df5a530208eb | 271 | if(current_left_motor_speed <= -0.7) |
| bbbobbbieo | 21:df5a530208eb | 272 | current_left_motor_speed= -0.7; |
| bbbobbbieo | 21:df5a530208eb | 273 | if(current_right_motor_speed <= -0.7) |
| bbbobbbieo | 21:df5a530208eb | 274 | current_right_motor_speed= -0.7; |
| bbbobbbieo | 16:11ba5d6f42ba | 275 | |
| bbbobbbieo | 14:f43b386b8b5d | 276 | TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed); |
| bbbobbbieo | 15:830209e846d5 | 277 | |
| bbbobbbieo | 14:f43b386b8b5d | 278 | } |
| bbbobbbieo | 14:f43b386b8b5d | 279 | |
| bbbobbbieo | 15:830209e846d5 | 280 | // clearing values for next image processing round |
| bbbobbbieo | 14:f43b386b8b5d | 281 | black_value_count = 0; |
| bbbobbbieo | 14:f43b386b8b5d | 282 | black_center_value = 0; |
| bbbobbbieo | 14:f43b386b8b5d | 283 | sum_black = 0; |
| bbbobbbieo | 14:f43b386b8b5d | 284 | |
| bbbobbbieo | 15:830209e846d5 | 285 | // end image processing |
| bbbobbbieo | 14:f43b386b8b5d | 286 | |
| bbbobbbieo | 14:f43b386b8b5d | 287 | linescan_ping_pong = false; |
| bbbobbbieo | 14:f43b386b8b5d | 288 | } // end checking channel 0 |
| bbbobbbieo | 15:830209e846d5 | 289 | |
| bbbobbbieo | 15:830209e846d5 | 290 | else { //checking channel 1 |
| bbbobbbieo | 14:f43b386b8b5d | 291 | linescan_ping_pong = true; |
| bbbobbbieo | 14:f43b386b8b5d | 292 | } |
| bbbobbbieo | 14:f43b386b8b5d | 293 | |
| bbbobbbieo | 7:455e7dd338ee | 294 | TFC_LineScanImageReady ==0; // since we used it, we reset the flag |
| bbbobbbieo | 14:f43b386b8b5d | 295 | }// end imageready |
| bbbobbbieo | 14:f43b386b8b5d | 296 | }// end linescan stuff |
| bbbobbbieo | 0:d57117b2188d | 297 | } |
| bbbobbbieo | 0:d57117b2188d | 298 | } |
| bbbobbbieo | 8:946806df7347 | 299 | |
| bbbobbbieo | 8:946806df7347 | 300 | |
| bbbobbbieo | 15:830209e846d5 | 301 | // shit code down here |