fatboyslim / Mbed 2 deprecated buttontest

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

main.cpp@26:3f7ddc7d5bdf, 2015-03-03 (annotated)

Committer:
mperella
Date:
Tue Mar 03 18:44:18 2015 +0000
Revision:
26:3f7ddc7d5bdf
Parent:
24:504a58daa06e
Child:
27:3dd1460365d3
new stuff

Who changed what in which revision?

UserRevisionLine numberNew 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 14:f43b386b8b5d 35 int violence_level = 0;
bbbobbbieo 15:830209e846d5 36
mperella 26:3f7ddc7d5bdf 37 int accelList[3];
mperella 26:3f7ddc7d5bdf 38 int lastAccessed = 0;
mperella 26:3f7ddc7d5bdf 39
mperella 26:3f7ddc7d5bdf 40 int centers_List[100];
mperella 26:3f7ddc7d5bdf 41
bbbobbbieo 20:c728b8ffad97 42 int center_now = 64;
bbbobbbieo 20:c728b8ffad97 43 int center_past_1 = 64;
bbbobbbieo 20:c728b8ffad97 44 int center_past_2 = 64;
bbbobbbieo 20:c728b8ffad97 45 int center_past_3 = 64;
bbbobbbieo 20:c728b8ffad97 46 int center_past_4 = 64;
bbbobbbieo 20:c728b8ffad97 47 //int best_guess_center = 64;
bbbobbbieo 20:c728b8ffad97 48
mperella 17:c643b6b4a96f 49 int set_point = 64;
bbbobbbieo 19:85eb7991e2ab 50 int previous_error = 0;
mperella 17:c643b6b4a96f 51 int error = 0;
mperella 17:c643b6b4a96f 52
mperella 17:c643b6b4a96f 53
bbbobbbieo 9:2b028ee421ad 54 //uint16_t MyImage0Buffer[2][128];
bbbobbbieo 9:2b028ee421ad 55 //uint16_t MyImage1Buffer[2][128];
bbbobbbieo 14:f43b386b8b5d 56
bbbobbbieo 7:455e7dd338ee 57 // major loop
bbbobbbieo 14:f43b386b8b5d 58 while(1) {
bbbobbbieo 14:f43b386b8b5d 59
bbbobbbieo 15:830209e846d5 60 // manual servo control, unused
bbbobbbieo 14:f43b386b8b5d 61 if (TFC_ReadPushButton(0) != 0 ) {
bbbobbbieo 14:f43b386b8b5d 62 current_servo_position = current_servo_position-.005;
bbbobbbieo 14:f43b386b8b5d 63 if(current_servo_position <= -0.4)
bbbobbbieo 14:f43b386b8b5d 64 current_servo_position = -0.4;
bbbobbbieo 14:f43b386b8b5d 65 TFC_SetServo(0, current_servo_position);
bbbobbbieo 14:f43b386b8b5d 66 }// end check button0
bbbobbbieo 14:f43b386b8b5d 67
bbbobbbieo 15:830209e846d5 68 else {}
bbbobbbieo 14:f43b386b8b5d 69
bbbobbbieo 15:830209e846d5 70 // manual servo control, unused
bbbobbbieo 14:f43b386b8b5d 71 if (TFC_ReadPushButton(1) != 0 ) {
bbbobbbieo 14:f43b386b8b5d 72 current_servo_position = current_servo_position+.005;
bbbobbbieo 14:f43b386b8b5d 73 if(current_servo_position >= 0.4)
bbbobbbieo 14:f43b386b8b5d 74 current_servo_position = 0.4;
bbbobbbieo 14:f43b386b8b5d 75 TFC_SetServo(0, current_servo_position);
bbbobbbieo 14:f43b386b8b5d 76 }// end check button1
bbbobbbieo 14:f43b386b8b5d 77
bbbobbbieo 15:830209e846d5 78 else {}
bbbobbbieo 14:f43b386b8b5d 79
bbbobbbieo 15:830209e846d5 80 // initial motor stuff
bbbobbbieo 14:f43b386b8b5d 81 if(rear_motor_enable_flag) {
bbbobbbieo 14:f43b386b8b5d 82 TFC_HBRIDGE_ENABLE;
bbbobbbieo 14:f43b386b8b5d 83
bbbobbbieo 15:830209e846d5 84 //current_left_motor_speed = (TFC_ReadPot(0));
bbbobbbieo 15:830209e846d5 85 //current_right_motor_speed = (TFC_ReadPot(1));
bbbobbbieo 14:f43b386b8b5d 86
bbbobbbieo 15:830209e846d5 87 // checking behavior level
bbbobbbieo 14:f43b386b8b5d 88 violence_level = int(TFC_GetDIP_Switch());
bbbobbbieo 14:f43b386b8b5d 89
bbbobbbieo 15:830209e846d5 90 if (violence_level==2) {
bbbobbbieo 16:11ba5d6f42ba 91 current_left_motor_speed = -.48;
bbbobbbieo 16:11ba5d6f42ba 92 current_right_motor_speed = .48;
bbbobbbieo 15:830209e846d5 93 }
bbbobbbieo 23:fe15b6147c1b 94 if (violence_level==3) {
bbbobbbieo 23:fe15b6147c1b 95 current_left_motor_speed = -.55;
bbbobbbieo 23:fe15b6147c1b 96 current_right_motor_speed = .55;
bbbobbbieo 23:fe15b6147c1b 97 }
bbbobbbieo 15:830209e846d5 98 else if (violence_level==1) {
bbbobbbieo 16:11ba5d6f42ba 99 current_left_motor_speed = -.35;
bbbobbbieo 16:11ba5d6f42ba 100 current_right_motor_speed = .35;
bbbobbbieo 14:f43b386b8b5d 101 }
bbbobbbieo 15:830209e846d5 102 else if (violence_level==0) {
bbbobbbieo 15:830209e846d5 103 current_left_motor_speed = 0;
bbbobbbieo 15:830209e846d5 104 current_right_motor_speed = 0;
bbbobbbieo 15:830209e846d5 105 }
bbbobbbieo 15:830209e846d5 106 else {
bbbobbbieo 15:830209e846d5 107 current_left_motor_speed = 0;
bbbobbbieo 15:830209e846d5 108 current_right_motor_speed = 0;
bbbobbbieo 15:830209e846d5 109 }
bbbobbbieo 15:830209e846d5 110
bbbobbbieo 14:f43b386b8b5d 111
bbbobbbieo 15:830209e846d5 112 // protection block
bbbobbbieo 15:830209e846d5 113 if(current_left_motor_speed >= 0.5)
bbbobbbieo 15:830209e846d5 114 current_left_motor_speed= 0.5;
bbbobbbieo 15:830209e846d5 115 if(current_right_motor_speed >= 0.5)
bbbobbbieo 15:830209e846d5 116 current_right_motor_speed= 0.5;
bbbobbbieo 15:830209e846d5 117 if(current_left_motor_speed <= -0.5)
bbbobbbieo 15:830209e846d5 118 current_left_motor_speed= -0.5;
bbbobbbieo 15:830209e846d5 119 if(current_right_motor_speed <= -0.5)
bbbobbbieo 15:830209e846d5 120 current_right_motor_speed= -0.5;
bbbobbbieo 14:f43b386b8b5d 121
bbbobbbieo 14:f43b386b8b5d 122 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
bbbobbbieo 14:f43b386b8b5d 123 }// end motor enabled
bbbobbbieo 14:f43b386b8b5d 124 else {
bbbobbbieo 14:f43b386b8b5d 125 TFC_HBRIDGE_DISABLE;
bbbobbbieo 14:f43b386b8b5d 126 }// end motor disabled
bbbobbbieo 14:f43b386b8b5d 127
bbbobbbieo 15:830209e846d5 128 // camera stuff
bbbobbbieo 14:f43b386b8b5d 129 if (linescan_enable) {
bbbobbbieo 14:f43b386b8b5d 130 if (TFC_LineScanImageReady !=0) {
bbbobbbieo 14:f43b386b8b5d 131
bbbobbbieo 15:830209e846d5 132 if (linescan_ping_pong) {
bbbobbbieo 15:830209e846d5 133 //checking channel 0
bbbobbbieo 15:830209e846d5 134
bbbobbbieo 15:830209e846d5 135 //checking center pixel, displays aprox value on leds
bbbobbbieo 14:f43b386b8b5d 136 uint8_t shitnum = 1;
bbbobbbieo 14:f43b386b8b5d 137 if (*(TFC_LineScanImage0+64) > 800)
bbbobbbieo 14:f43b386b8b5d 138 shitnum = 15;
bbbobbbieo 14:f43b386b8b5d 139 else if((*(TFC_LineScanImage0+64) > 450))
bbbobbbieo 14:f43b386b8b5d 140 shitnum = 7;
bbbobbbieo 14:f43b386b8b5d 141 else if((*(TFC_LineScanImage0+64) > 400))
bbbobbbieo 14:f43b386b8b5d 142 shitnum = 3;
bbbobbbieo 14:f43b386b8b5d 143 else
bbbobbbieo 14:f43b386b8b5d 144 shitnum = 1;
bbbobbbieo 14:f43b386b8b5d 145 TFC_SetBatteryLED(shitnum);
bbbobbbieo 14:f43b386b8b5d 146
bbbobbbieo 15:830209e846d5 147
bbbobbbieo 15:830209e846d5 148 // checking for center line (single line)
bbbobbbieo 14:f43b386b8b5d 149 for (uint16_t i=0; i<128; i++) {
bbbobbbieo 14:f43b386b8b5d 150 if ((*(TFC_LineScanImage0+i) < 300)) {
bbbobbbieo 14:f43b386b8b5d 151 black_values_list[black_value_count] = i;
bbbobbbieo 14:f43b386b8b5d 152 black_value_count++;
bbbobbbieo 14:f43b386b8b5d 153 }
bbbobbbieo 14:f43b386b8b5d 154 }
bbbobbbieo 14:f43b386b8b5d 155
bbbobbbieo 14:f43b386b8b5d 156 for(int i=0; i<black_value_count; i++) {
bbbobbbieo 14:f43b386b8b5d 157 sum_black += black_values_list[i];
bbbobbbieo 14:f43b386b8b5d 158 }
bbbobbbieo 14:f43b386b8b5d 159
bbbobbbieo 20:c728b8ffad97 160 //update history
bbbobbbieo 20:c728b8ffad97 161 center_past_4= center_past_3;
bbbobbbieo 20:c728b8ffad97 162 center_past_3= center_past_2;
bbbobbbieo 20:c728b8ffad97 163 center_past_2= center_past_1;
bbbobbbieo 20:c728b8ffad97 164 center_past_1= center_now;
mperella 26:3f7ddc7d5bdf 165
mperella 26:3f7ddc7d5bdf 166 /*
mperella 26:3f7ddc7d5bdf 167
mperella 26:3f7ddc7d5bdf 168 for(int i = 99; i >= 0; i--)
mperella 26:3f7ddc7d5bdf 169 {
mperella 26:3f7ddc7d5bdf 170 if(i = 0)
mperella 26:3f7ddc7d5bdf 171 {
mperella 26:3f7ddc7d5bdf 172 centers_List[i] = center_now;
mperella 26:3f7ddc7d5bdf 173 }
mperella 26:3f7ddc7d5bdf 174 else
mperella 26:3f7ddc7d5bdf 175 {
mperella 26:3f7ddc7d5bdf 176 centers_List[i] = centers_List[i-1];
mperella 26:3f7ddc7d5bdf 177 }
mperella 26:3f7ddc7d5bdf 178 }
mperella 26:3f7ddc7d5bdf 179
mperella 26:3f7ddc7d5bdf 180 */
mperella 26:3f7ddc7d5bdf 181
bbbobbbieo 20:c728b8ffad97 182
bbbobbbieo 15:830209e846d5 183 // value of center of black (single line)
bbbobbbieo 20:c728b8ffad97 184 //black_center_value = sum_black / black_value_count;
bbbobbbieo 20:c728b8ffad97 185 center_now = sum_black / black_value_count;
bbbobbbieo 20:c728b8ffad97 186
bbbobbbieo 20:c728b8ffad97 187 // best guess of center based on weighted average of history
bbbobbbieo 24:504a58daa06e 188 //black_center_value = (5*center_now + 10*center_past_1 + 15*center_past_2 +30*center_past_3 +40*center_past_4)/100;
bbbobbbieo 24:504a58daa06e 189 black_center_value = (30*center_now + 10*center_past_1 + 15*center_past_2 +5*center_past_3 +40*center_past_4)/100;
bbbobbbieo 15:830209e846d5 190
mperella 17:c643b6b4a96f 191 /* ******* PID ALGORITHM *******
mperella 17:c643b6b4a96f 192
mperella 17:c643b6b4a96f 193 error = set_point - black_center_value;
mperella 17:c643b6b4a96f 194 integral = integral + error*dt;
mperella 17:c643b6b4a96f 195 derivative = (error - previous_error)/dt;
mperella 17:c643b6b4a96f 196 output = kp*error + ki*integral + kd*derivative;
mperella 17:c643b6b4a96f 197 previous error = error;
mperella 17:c643b6b4a96f 198
mperella 17:c643b6b4a96f 199 GOTTA DO SOME TESTS/SIMULATIONS TO CALIBRATE SERVO ADJUSTMENTS
mperella 17:c643b6b4a96f 200
mperella 17:c643b6b4a96f 201
mperella 17:c643b6b4a96f 202 *****************************
mperella 17:c643b6b4a96f 203 */
bbbobbbieo 14:f43b386b8b5d 204
bbbobbbieo 15:830209e846d5 205 // need to turn left
bbbobbbieo 15:830209e846d5 206 if (black_center_value < 64) {
bbbobbbieo 15:830209e846d5 207
bbbobbbieo 16:11ba5d6f42ba 208 current_servo_position= float(.01875*black_center_value-(1.2));
bbbobbbieo 14:f43b386b8b5d 209 if(current_servo_position <= -0.4)
bbbobbbieo 14:f43b386b8b5d 210 current_servo_position = -0.4;
bbbobbbieo 14:f43b386b8b5d 211 TFC_SetServo(0, current_servo_position);
mperella 26:3f7ddc7d5bdf 212
mperella 26:3f7ddc7d5bdf 213 boolean listSame = true;
mperella 26:3f7ddc7d5bdf 214
mperella 26:3f7ddc7d5bdf 215 /*
mperella 26:3f7ddc7d5bdf 216 if(center_now + 5 < center_past_4 && center_now - 5 > center_past_4)
mperella 26:3f7ddc7d5bdf 217 {
mperella 26:3f7ddc7d5bdf 218 accelList[lastAccessed] = 1;
mperella 26:3f7ddc7d5bdf 219 if(lastAccessed != 2)
mperella 26:3f7ddc7d5bdf 220 {
mperella 26:3f7ddc7d5bdf 221 lastAccessed = lastAccessed + 1;
mperella 26:3f7ddc7d5bdf 222 }
mperella 26:3f7ddc7d5bdf 223 else
mperella 26:3f7ddc7d5bdf 224 {
mperella 26:3f7ddc7d5bdf 225 lastAccessed = 0;
mperella 26:3f7ddc7d5bdf 226 }
mperella 26:3f7ddc7d5bdf 227 }
mperella 26:3f7ddc7d5bdf 228
mperella 26:3f7ddc7d5bdf 229
mperella 26:3f7ddc7d5bdf 230
mperella 26:3f7ddc7d5bdf 231 for(int i = 0; i < 3; i++)
mperella 26:3f7ddc7d5bdf 232 {
mperella 26:3f7ddc7d5bdf 233 while(listSame)
mperella 26:3f7ddc7d5bdf 234 {
mperella 26:3f7ddc7d5bdf 235 if(accelList[i] > 0)
mperella 26:3f7ddc7d5bdf 236 {
mperella 26:3f7ddc7d5bdf 237
mperella 26:3f7ddc7d5bdf 238 }
mperella 26:3f7ddc7d5bdf 239 else
mperella 26:3f7ddc7d5bdf 240 {
mperella 26:3f7ddc7d5bdf 241 listSame = false;
mperella 26:3f7ddc7d5bdf 242 }
mperella 26:3f7ddc7d5bdf 243 }
mperella 26:3f7ddc7d5bdf 244 }
mperella 26:3f7ddc7d5bdf 245
mperella 26:3f7ddc7d5bdf 246 if(listSame)
mperella 26:3f7ddc7d5bdf 247 {
mperella 26:3f7ddc7d5bdf 248 current_left_motor_speed = current_left_motor_speed + .010;
mperella 26:3f7ddc7d5bdf 249 current_right_motor_speed = current_right_motor_speed + .010;
mperella 26:3f7ddc7d5bdf 250 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
mperella 26:3f7ddc7d5bdf 251 }
mperella 26:3f7ddc7d5bdf 252
mperella 26:3f7ddc7d5bdf 253
mperella 26:3f7ddc7d5bdf 254 */
bbbobbbieo 15:830209e846d5 255
bbbobbbieo 16:11ba5d6f42ba 256 //current_left_motor_speed = current_left_motor_speed + float(64-black_center_value)*.0025;
bbbobbbieo 16:11ba5d6f42ba 257 //current_right_motor_speed = current_right_motor_speed + float(64-black_center_value)*.0025;
bbbobbbieo 16:11ba5d6f42ba 258 if (violence_level !=0){
bbbobbbieo 19:85eb7991e2ab 259 current_left_motor_speed = current_left_motor_speed + float(float(64-black_center_value)*.025);// kinda reverse this...
bbbobbbieo 16:11ba5d6f42ba 260 current_right_motor_speed = current_right_motor_speed + float(float(64-black_center_value)*.045);// push more forwards
bbbobbbieo 16:11ba5d6f42ba 261 }
bbbobbbieo 16:11ba5d6f42ba 262
bbbobbbieo 16:11ba5d6f42ba 263 // protection block
bbbobbbieo 16:11ba5d6f42ba 264 if(current_left_motor_speed >= 0.5)
bbbobbbieo 16:11ba5d6f42ba 265 current_left_motor_speed= 0.5;
bbbobbbieo 16:11ba5d6f42ba 266 if(current_right_motor_speed >= 0.5)
bbbobbbieo 16:11ba5d6f42ba 267 current_right_motor_speed= 0.5;
bbbobbbieo 16:11ba5d6f42ba 268 if(current_left_motor_speed <= -0.5)
bbbobbbieo 16:11ba5d6f42ba 269 current_left_motor_speed= -0.5;
bbbobbbieo 16:11ba5d6f42ba 270 if(current_right_motor_speed <= -0.5)
bbbobbbieo 16:11ba5d6f42ba 271 current_right_motor_speed= -0.5;
bbbobbbieo 16:11ba5d6f42ba 272
bbbobbbieo 14:f43b386b8b5d 273 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
bbbobbbieo 14:f43b386b8b5d 274
bbbobbbieo 7:455e7dd338ee 275 }
bbbobbbieo 14:f43b386b8b5d 276
bbbobbbieo 15:830209e846d5 277 // need to turn right
bbbobbbieo 14:f43b386b8b5d 278 if (black_center_value > 64) {
bbbobbbieo 14:f43b386b8b5d 279
bbbobbbieo 16:11ba5d6f42ba 280 current_servo_position= float(.01875*black_center_value-(1.2));
bbbobbbieo 14:f43b386b8b5d 281 if( current_servo_position >= +0.4)
bbbobbbieo 14:f43b386b8b5d 282 current_servo_position = +0.4;
bbbobbbieo 14:f43b386b8b5d 283 TFC_SetServo(0, current_servo_position);
mperella 26:3f7ddc7d5bdf 284
mperella 26:3f7ddc7d5bdf 285 boolean listSame = true;
mperella 26:3f7ddc7d5bdf 286
mperella 26:3f7ddc7d5bdf 287 /*
mperella 26:3f7ddc7d5bdf 288 if(center_now + 5 < center_past_4 && center_now - 5 > center_past_4)
mperella 26:3f7ddc7d5bdf 289 {
mperella 26:3f7ddc7d5bdf 290 accelList[lastAccessed] = 1;
mperella 26:3f7ddc7d5bdf 291 if(lastAccessed != 2)
mperella 26:3f7ddc7d5bdf 292 {
mperella 26:3f7ddc7d5bdf 293 lastAccessed = lastAccessed + 1;
mperella 26:3f7ddc7d5bdf 294 }
mperella 26:3f7ddc7d5bdf 295 else
mperella 26:3f7ddc7d5bdf 296 {
mperella 26:3f7ddc7d5bdf 297 lastAccessed = 0;
mperella 26:3f7ddc7d5bdf 298 }
mperella 26:3f7ddc7d5bdf 299 }
mperella 26:3f7ddc7d5bdf 300
mperella 26:3f7ddc7d5bdf 301
mperella 26:3f7ddc7d5bdf 302
mperella 26:3f7ddc7d5bdf 303 for(int i = 0; i < 3; i++)
mperella 26:3f7ddc7d5bdf 304 {
mperella 26:3f7ddc7d5bdf 305 while(listSame)
mperella 26:3f7ddc7d5bdf 306 {
mperella 26:3f7ddc7d5bdf 307 if(accelList[i] > 0)
mperella 26:3f7ddc7d5bdf 308 {
mperella 26:3f7ddc7d5bdf 309
mperella 26:3f7ddc7d5bdf 310 }
mperella 26:3f7ddc7d5bdf 311 else
mperella 26:3f7ddc7d5bdf 312 {
mperella 26:3f7ddc7d5bdf 313 listSame = false;
mperella 26:3f7ddc7d5bdf 314 }
mperella 26:3f7ddc7d5bdf 315 }
mperella 26:3f7ddc7d5bdf 316 }
mperella 26:3f7ddc7d5bdf 317
mperella 26:3f7ddc7d5bdf 318 if(listSame)
mperella 26:3f7ddc7d5bdf 319 {
mperella 26:3f7ddc7d5bdf 320 current_left_motor_speed = current_left_motor_speed + .010;
mperella 26:3f7ddc7d5bdf 321 current_right_motor_speed = current_right_motor_speed + .010;
mperella 26:3f7ddc7d5bdf 322 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
mperella 26:3f7ddc7d5bdf 323 }
mperella 26:3f7ddc7d5bdf 324
mperella 26:3f7ddc7d5bdf 325
mperella 26:3f7ddc7d5bdf 326 */
bbbobbbieo 15:830209e846d5 327
bbbobbbieo 16:11ba5d6f42ba 328 //current_left_motor_speed = current_left_motor_speed - float(black_center_value-64)*.0025;
bbbobbbieo 16:11ba5d6f42ba 329 //current_right_motor_speed = current_right_motor_speed - float(black_center_value-64)*.0025;
bbbobbbieo 16:11ba5d6f42ba 330 if (violence_level !=0){
bbbobbbieo 16:11ba5d6f42ba 331 current_left_motor_speed = current_left_motor_speed - float(float(black_center_value-64)*.045);// push more forwards
bbbobbbieo 19:85eb7991e2ab 332 current_right_motor_speed = current_right_motor_speed - float(float(black_center_value-64)*.025);// kinda reverse this...
bbbobbbieo 16:11ba5d6f42ba 333 }
bbbobbbieo 15:830209e846d5 334
bbbobbbieo 16:11ba5d6f42ba 335 // protection block
bbbobbbieo 16:11ba5d6f42ba 336 if(current_left_motor_speed >= 0.5)
bbbobbbieo 16:11ba5d6f42ba 337 current_left_motor_speed= 0.5;
bbbobbbieo 16:11ba5d6f42ba 338 if(current_right_motor_speed >= 0.5)
bbbobbbieo 16:11ba5d6f42ba 339 current_right_motor_speed= 0.5;
bbbobbbieo 16:11ba5d6f42ba 340 if(current_left_motor_speed <= -0.5)
bbbobbbieo 16:11ba5d6f42ba 341 current_left_motor_speed= -0.5;
bbbobbbieo 16:11ba5d6f42ba 342 if(current_right_motor_speed <= -0.5)
bbbobbbieo 16:11ba5d6f42ba 343 current_right_motor_speed= -0.5;
bbbobbbieo 16:11ba5d6f42ba 344
bbbobbbieo 14:f43b386b8b5d 345 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
bbbobbbieo 15:830209e846d5 346
bbbobbbieo 14:f43b386b8b5d 347 }
bbbobbbieo 14:f43b386b8b5d 348
bbbobbbieo 15:830209e846d5 349 // clearing values for next image processing round
bbbobbbieo 14:f43b386b8b5d 350 black_value_count = 0;
bbbobbbieo 14:f43b386b8b5d 351 black_center_value = 0;
bbbobbbieo 14:f43b386b8b5d 352 sum_black = 0;
bbbobbbieo 14:f43b386b8b5d 353
bbbobbbieo 15:830209e846d5 354 // end image processing
bbbobbbieo 14:f43b386b8b5d 355
bbbobbbieo 14:f43b386b8b5d 356 linescan_ping_pong = false;
bbbobbbieo 14:f43b386b8b5d 357 } // end checking channel 0
bbbobbbieo 15:830209e846d5 358
bbbobbbieo 15:830209e846d5 359 else { //checking channel 1
bbbobbbieo 14:f43b386b8b5d 360 linescan_ping_pong = true;
bbbobbbieo 14:f43b386b8b5d 361 }
bbbobbbieo 14:f43b386b8b5d 362
bbbobbbieo 7:455e7dd338ee 363 TFC_LineScanImageReady ==0; // since we used it, we reset the flag
bbbobbbieo 14:f43b386b8b5d 364 }// end imageready
bbbobbbieo 14:f43b386b8b5d 365 }// end linescan stuff
bbbobbbieo 0:d57117b2188d 366 }
bbbobbbieo 0:d57117b2188d 367 }
bbbobbbieo 8:946806df7347 368
bbbobbbieo 8:946806df7347 369
bbbobbbieo 15:830209e846d5 370 // shit code down here