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@39:4a0803d7575d, 2015-03-05 (annotated)

Committer:
bbbobbbieo
Date:
Thu Mar 05 20:18:49 2015 +0000
Revision:
39:4a0803d7575d
Parent:
38:ecb6da94cb64
Child:
40:cdb079fc9d0a
bullshit, need to go back to linear for now

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"
mperella 37:9c544f53563d 3 #include <iostream>
mperella 37:9c544f53563d 4 #include <stdio.h>
bbbobbbieo 38:ecb6da94cb64 5 //#include "serialib.h"
bbbobbbieo 0:d57117b2188d 6
bbbobbbieo 39:4a0803d7575d 7 const float AGGRESSIVE = .55;
bbbobbbieo 39:4a0803d7575d 8 const float MODERATE =.48;
bbbobbbieo 39:4a0803d7575d 9 const float CONSERVATIVE =.35;
bbbobbbieo 39:4a0803d7575d 10 const float STOP =0;
bbbobbbieo 39:4a0803d7575d 11 const float PROTECTION_THRESHOLD_UPPER =.7;
bbbobbbieo 39:4a0803d7575d 12 const float PROTECTION_THRESHOLD_LOWER =-.7;
bbbobbbieo 39:4a0803d7575d 13 const float TURN_FORWARD_ACCEL =0.045;
bbbobbbieo 39:4a0803d7575d 14 const float TURN_BACKWARD_ACCEL =0.025;
bbbobbbieo 39:4a0803d7575d 15 const float SERVO_CAN_MOVE_IN_ONE_FRAME =0.1;
bbbobbbieo 39:4a0803d7575d 16 const float SERVO_MAX =.5;
bbbobbbieo 39:4a0803d7575d 17
bbbobbbieo 39:4a0803d7575d 18 const int BLACK_THRESHOLD =63;
bbbobbbieo 39:4a0803d7575d 19 const int LINE_SCAN_LENGTH =128;
bbbobbbieo 39:4a0803d7575d 20
bbergandy 25:1d0f586aaf0c 21
bbbobbbieo 0:d57117b2188d 22 DigitalOut myled(LED1);
bbbobbbieo 0:d57117b2188d 23
bbbobbbieo 14:f43b386b8b5d 24 int main()
bbbobbbieo 1:21d40d90b2f0 25 {
bbbobbbieo 7:455e7dd338ee 26 //run this before anything
bbbobbbieo 2:d8a51492b646 27 TFC_Init();
bbbobbbieo 14:f43b386b8b5d 28
bbbobbbieo 7:455e7dd338ee 29 //variables
bbbobbbieo 4:7584ff0426f1 30 float current_servo_position = 0;
bbbobbbieo 38:ecb6da94cb64 31 float previous_servo_position = 0;
bbbobbbieo 6:44d1079f076c 32 float current_left_motor_speed = 0;
bbbobbbieo 6:44d1079f076c 33 float current_right_motor_speed = 0;
mperella 17:c643b6b4a96f 34
mperella 17:c643b6b4a96f 35 float dt = 0.00001;
mperella 17:c643b6b4a96f 36 float integral = 0;
mperella 17:c643b6b4a96f 37 float derivative = 0;
mperella 17:c643b6b4a96f 38 float output = 0;
mperella 17:c643b6b4a96f 39
mperella 17:c643b6b4a96f 40 // gains on prop, int, der
mperella 17:c643b6b4a96f 41 // subject to change, need to fine tune
mperella 17:c643b6b4a96f 42 float kp = 1.8960;
mperella 17:c643b6b4a96f 43 float ki = 0.6170;
mperella 17:c643b6b4a96f 44 float kd = 1.5590;
mperella 17:c643b6b4a96f 45
bbbobbbieo 6:44d1079f076c 46 bool rear_motor_enable_flag = true;
bbbobbbieo 7:455e7dd338ee 47 bool linescan_ping_pong = false;
bbbobbbieo 9:2b028ee421ad 48 bool linescan_enable = true;
bbbobbbieo 14:f43b386b8b5d 49
bbergandy 31:0c2b49175036 50 int black_values_list[LINE_SCAN_LENGTH];
bbbobbbieo 11:d65d6d7fc85e 51 int black_value_count = 0;
bbbobbbieo 11:d65d6d7fc85e 52 int black_center_value = 0;
bbbobbbieo 11:d65d6d7fc85e 53 int sum_black = 0;
bbbobbbieo 14:f43b386b8b5d 54 int violence_level = 0;
bbbobbbieo 15:830209e846d5 55
mperella 26:3f7ddc7d5bdf 56 int accelList[3];
mperella 26:3f7ddc7d5bdf 57 int lastAccessed = 0;
mperella 26:3f7ddc7d5bdf 58
mperella 28:06cefc8dd15e 59 int centers_List[50];
mperella 26:3f7ddc7d5bdf 60
mperella 28:06cefc8dd15e 61 int center_now = 63;
mperella 28:06cefc8dd15e 62 int center_past_1 = 63;
mperella 28:06cefc8dd15e 63 int center_past_2 = 63;
mperella 28:06cefc8dd15e 64 int center_past_3 = 63;
mperella 28:06cefc8dd15e 65 int center_past_4 = 63;
bbbobbbieo 20:c728b8ffad97 66 //int best_guess_center = 64;
bbbobbbieo 20:c728b8ffad97 67
mperella 28:06cefc8dd15e 68 int set_point = 63;
bbbobbbieo 19:85eb7991e2ab 69 int previous_error = 0;
mperella 17:c643b6b4a96f 70 int error = 0;
mperella 17:c643b6b4a96f 71
mperella 28:06cefc8dd15e 72 for(int i = 0; i < 50; i++)
mperella 28:06cefc8dd15e 73 centers_List[i] = 63;
mperella 17:c643b6b4a96f 74
bbbobbbieo 9:2b028ee421ad 75 //uint16_t MyImage0Buffer[2][128];
bbbobbbieo 9:2b028ee421ad 76 //uint16_t MyImage1Buffer[2][128];
bbbobbbieo 14:f43b386b8b5d 77
bbbobbbieo 7:455e7dd338ee 78 // major loop
bbbobbbieo 14:f43b386b8b5d 79 while(1) {
bbbobbbieo 14:f43b386b8b5d 80
bbbobbbieo 15:830209e846d5 81 // manual servo control, unused
bbbobbbieo 14:f43b386b8b5d 82 if (TFC_ReadPushButton(0) != 0 ) {
bbbobbbieo 14:f43b386b8b5d 83 current_servo_position = current_servo_position-.005;
bbbobbbieo 14:f43b386b8b5d 84 if(current_servo_position <= -0.4)
bbbobbbieo 14:f43b386b8b5d 85 current_servo_position = -0.4;
bbbobbbieo 14:f43b386b8b5d 86 TFC_SetServo(0, current_servo_position);
bbbobbbieo 14:f43b386b8b5d 87 }// end check button0
bbbobbbieo 14:f43b386b8b5d 88
bbbobbbieo 15:830209e846d5 89 else {}
mperella 28:06cefc8dd15e 90
bbbobbbieo 14:f43b386b8b5d 91
bbbobbbieo 15:830209e846d5 92 // manual servo control, unused
bbbobbbieo 14:f43b386b8b5d 93 if (TFC_ReadPushButton(1) != 0 ) {
bbbobbbieo 14:f43b386b8b5d 94 current_servo_position = current_servo_position+.005;
bbbobbbieo 14:f43b386b8b5d 95 if(current_servo_position >= 0.4)
bbbobbbieo 14:f43b386b8b5d 96 current_servo_position = 0.4;
bbbobbbieo 14:f43b386b8b5d 97 TFC_SetServo(0, current_servo_position);
bbbobbbieo 14:f43b386b8b5d 98 }// end check button1
bbbobbbieo 14:f43b386b8b5d 99
bbbobbbieo 15:830209e846d5 100 else {}
bbbobbbieo 14:f43b386b8b5d 101
bbbobbbieo 15:830209e846d5 102 // initial motor stuff
bbbobbbieo 14:f43b386b8b5d 103 if(rear_motor_enable_flag) {
bbbobbbieo 14:f43b386b8b5d 104 TFC_HBRIDGE_ENABLE;
bbbobbbieo 14:f43b386b8b5d 105
bbbobbbieo 15:830209e846d5 106 //current_left_motor_speed = (TFC_ReadPot(0));
bbbobbbieo 15:830209e846d5 107 //current_right_motor_speed = (TFC_ReadPot(1));
bbbobbbieo 14:f43b386b8b5d 108
bbbobbbieo 15:830209e846d5 109 // checking behavior level
bbbobbbieo 14:f43b386b8b5d 110 violence_level = int(TFC_GetDIP_Switch());
bbbobbbieo 14:f43b386b8b5d 111
bbergandy 25:1d0f586aaf0c 112 if (violence_level==3) {
bbergandy 25:1d0f586aaf0c 113 current_left_motor_speed = -(AGGRESSIVE);
bbergandy 25:1d0f586aaf0c 114 current_right_motor_speed = AGGRESSIVE;
bbbobbbieo 15:830209e846d5 115 }
bbergandy 36:3491a7d8ffb6 116 else if (violence_level==2) {
bbergandy 25:1d0f586aaf0c 117 current_left_motor_speed = -(MODERATE);
bbergandy 25:1d0f586aaf0c 118 current_right_motor_speed = (MODERATE);
bbbobbbieo 23:fe15b6147c1b 119 }
bbbobbbieo 15:830209e846d5 120 else if (violence_level==1) {
bbergandy 25:1d0f586aaf0c 121 current_left_motor_speed = -(CONSERVATIVE);
bbergandy 25:1d0f586aaf0c 122 current_right_motor_speed = CONSERVATIVE;
bbbobbbieo 14:f43b386b8b5d 123 }
bbbobbbieo 15:830209e846d5 124 else if (violence_level==0) {
bbergandy 25:1d0f586aaf0c 125 current_left_motor_speed = STOP;
bbergandy 25:1d0f586aaf0c 126 current_right_motor_speed = STOP;
bbbobbbieo 15:830209e846d5 127 }
bbbobbbieo 15:830209e846d5 128 else {
bbergandy 25:1d0f586aaf0c 129 current_left_motor_speed = STOP;
bbergandy 25:1d0f586aaf0c 130 current_right_motor_speed = STOP;
bbbobbbieo 15:830209e846d5 131 }
bbbobbbieo 15:830209e846d5 132
bbbobbbieo 14:f43b386b8b5d 133
bbbobbbieo 15:830209e846d5 134 // protection block
bbergandy 31:0c2b49175036 135 if(current_left_motor_speed >= PROTECTION_THRESHOLD_UPPER)
bbergandy 31:0c2b49175036 136 current_left_motor_speed= PROTECTION_THRESHOLD_UPPER;
bbergandy 31:0c2b49175036 137 if(current_right_motor_speed >= PROTECTION_THRESHOLD_UPPER)
bbergandy 31:0c2b49175036 138 current_right_motor_speed = PROTECTION_THRESHOLD_UPPER;
bbergandy 31:0c2b49175036 139 if(current_left_motor_speed <= PROTECTION_THRESHOLD_LOWER)
bbergandy 31:0c2b49175036 140 current_left_motor_speed = PROTECTION_THRESHOLD_LOWER;
bbergandy 31:0c2b49175036 141 if(current_right_motor_speed <= PROTECTION_THRESHOLD_LOWER)
bbergandy 31:0c2b49175036 142 current_right_motor_speed = PROTECTION_THRESHOLD_LOWER;
bbbobbbieo 14:f43b386b8b5d 143
bbbobbbieo 14:f43b386b8b5d 144 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
bbbobbbieo 14:f43b386b8b5d 145 }// end motor enabled
bbbobbbieo 14:f43b386b8b5d 146 else {
bbbobbbieo 14:f43b386b8b5d 147 TFC_HBRIDGE_DISABLE;
bbbobbbieo 14:f43b386b8b5d 148 }// end motor disabled
bbbobbbieo 14:f43b386b8b5d 149
bbbobbbieo 15:830209e846d5 150 // camera stuff
bbbobbbieo 14:f43b386b8b5d 151 if (linescan_enable) {
bbbobbbieo 14:f43b386b8b5d 152 if (TFC_LineScanImageReady !=0) {
bbbobbbieo 14:f43b386b8b5d 153
bbbobbbieo 15:830209e846d5 154 if (linescan_ping_pong) {
bbbobbbieo 15:830209e846d5 155 //checking channel 0
bbbobbbieo 15:830209e846d5 156
bbbobbbieo 15:830209e846d5 157 //checking center pixel, displays aprox value on leds
bbbobbbieo 14:f43b386b8b5d 158 uint8_t shitnum = 1;
bbergandy 31:0c2b49175036 159
mperella 37:9c544f53563d 160 /*
bbbobbbieo 14:f43b386b8b5d 161 if (*(TFC_LineScanImage0+64) > 800)
bbbobbbieo 14:f43b386b8b5d 162 shitnum = 15;
bbbobbbieo 14:f43b386b8b5d 163 else if((*(TFC_LineScanImage0+64) > 450))
bbbobbbieo 14:f43b386b8b5d 164 shitnum = 7;
bbbobbbieo 14:f43b386b8b5d 165 else if((*(TFC_LineScanImage0+64) > 400))
bbbobbbieo 14:f43b386b8b5d 166 shitnum = 3;
bbbobbbieo 14:f43b386b8b5d 167 else
bbbobbbieo 14:f43b386b8b5d 168 shitnum = 1;
bbbobbbieo 14:f43b386b8b5d 169 TFC_SetBatteryLED(shitnum);
mperella 37:9c544f53563d 170 */
bbbobbbieo 15:830209e846d5 171
bbbobbbieo 15:830209e846d5 172 // checking for center line (single line)
bbbobbbieo 14:f43b386b8b5d 173 for (uint16_t i=0; i<128; i++) {
bbbobbbieo 38:ecb6da94cb64 174 if ((*(TFC_LineScanImage0+i) < 250)) {
bbbobbbieo 14:f43b386b8b5d 175 black_values_list[black_value_count] = i;
bbbobbbieo 14:f43b386b8b5d 176 black_value_count++;
bbbobbbieo 14:f43b386b8b5d 177 }
bbbobbbieo 14:f43b386b8b5d 178 }
bbbobbbieo 14:f43b386b8b5d 179
bbbobbbieo 14:f43b386b8b5d 180 for(int i=0; i<black_value_count; i++) {
bbbobbbieo 14:f43b386b8b5d 181 sum_black += black_values_list[i];
bbbobbbieo 14:f43b386b8b5d 182 }
bbbobbbieo 14:f43b386b8b5d 183
bbbobbbieo 20:c728b8ffad97 184 //update history
bbbobbbieo 20:c728b8ffad97 185 center_past_4= center_past_3;
bbbobbbieo 20:c728b8ffad97 186 center_past_3= center_past_2;
bbbobbbieo 20:c728b8ffad97 187 center_past_2= center_past_1;
bbbobbbieo 20:c728b8ffad97 188 center_past_1= center_now;
mperella 26:3f7ddc7d5bdf 189
bbergandy 36:3491a7d8ffb6 190 /*
mperella 28:06cefc8dd15e 191 for(int i = 49; i >= 0; i--)
mperella 26:3f7ddc7d5bdf 192 {
mperella 26:3f7ddc7d5bdf 193 if(i = 0)
mperella 26:3f7ddc7d5bdf 194 {
mperella 26:3f7ddc7d5bdf 195 centers_List[i] = center_now;
mperella 26:3f7ddc7d5bdf 196 }
mperella 26:3f7ddc7d5bdf 197 else
mperella 26:3f7ddc7d5bdf 198 {
mperella 26:3f7ddc7d5bdf 199 centers_List[i] = centers_List[i-1];
mperella 26:3f7ddc7d5bdf 200 }
bbergandy 36:3491a7d8ffb6 201 }
bbergandy 36:3491a7d8ffb6 202 */
bbbobbbieo 20:c728b8ffad97 203
bbbobbbieo 15:830209e846d5 204 // value of center of black (single line)
bbbobbbieo 20:c728b8ffad97 205 //black_center_value = sum_black / black_value_count;
bbbobbbieo 38:ecb6da94cb64 206
bbbobbbieo 39:4a0803d7575d 207 if (black_value_count>2)
bbbobbbieo 20:c728b8ffad97 208 center_now = sum_black / black_value_count;
bbbobbbieo 20:c728b8ffad97 209
mperella 37:9c544f53563d 210 uint8_t num = 0;
mperella 37:9c544f53563d 211
mperella 37:9c544f53563d 212 if(center_now > 0 && center_now < 27)
mperella 37:9c544f53563d 213 num = 1;
mperella 37:9c544f53563d 214 else if(center_now >= 27 && center_now < 54)
mperella 37:9c544f53563d 215 num = 2;
mperella 37:9c544f53563d 216 else if(center_now >= 54 && center_now < 81)
mperella 37:9c544f53563d 217 num = 4;
mperella 37:9c544f53563d 218 else if(center_now >= 81 && center_now < 127)
mperella 37:9c544f53563d 219 num = 8;
mperella 37:9c544f53563d 220 else if(center_now > 50 && center_now < 80)
mperella 37:9c544f53563d 221 num = 15;
mperella 37:9c544f53563d 222 else
mperella 37:9c544f53563d 223 num = 0;
mperella 37:9c544f53563d 224
mperella 37:9c544f53563d 225 TFC_SetBatteryLED(num);
mperella 37:9c544f53563d 226
bbbobbbieo 20:c728b8ffad97 227 // best guess of center based on weighted average of history
bbbobbbieo 24:504a58daa06e 228 //black_center_value = (5*center_now + 10*center_past_1 + 15*center_past_2 +30*center_past_3 +40*center_past_4)/100;
bbergandy 36:3491a7d8ffb6 229 black_center_value = (15*center_now + 15*center_past_1 + 15*center_past_2 +25*center_past_3 +30*center_past_4)/100;
bbbobbbieo 15:830209e846d5 230
mperella 17:c643b6b4a96f 231 /* ******* PID ALGORITHM *******
mperella 17:c643b6b4a96f 232
mperella 17:c643b6b4a96f 233 error = set_point - black_center_value;
mperella 17:c643b6b4a96f 234 integral = integral + error*dt;
mperella 17:c643b6b4a96f 235 derivative = (error - previous_error)/dt;
mperella 17:c643b6b4a96f 236 output = kp*error + ki*integral + kd*derivative;
mperella 17:c643b6b4a96f 237 previous error = error;
mperella 17:c643b6b4a96f 238
mperella 17:c643b6b4a96f 239 GOTTA DO SOME TESTS/SIMULATIONS TO CALIBRATE SERVO ADJUSTMENTS
mperella 17:c643b6b4a96f 240
mperella 17:c643b6b4a96f 241
mperella 17:c643b6b4a96f 242 *****************************
mperella 17:c643b6b4a96f 243 */
mperella 28:06cefc8dd15e 244
bbergandy 36:3491a7d8ffb6 245 /*
mperella 28:06cefc8dd15e 246 if (black_center_value == BLACK_THRESHOLD)
mperella 28:06cefc8dd15e 247 {
mperella 28:06cefc8dd15e 248 TFC_SetServo(0,0);
mperella 28:06cefc8dd15e 249
mperella 29:91bda3a63f17 250 int temp = 0;
mperella 29:91bda3a63f17 251 int temp2 = 0;
mperella 28:06cefc8dd15e 252
mperella 28:06cefc8dd15e 253 for(int i = 20; i < 30; i++)
mperella 28:06cefc8dd15e 254 {
mperella 28:06cefc8dd15e 255 temp = centers_List[i] + temp;
mperella 28:06cefc8dd15e 256 }
mperella 28:06cefc8dd15e 257
mperella 28:06cefc8dd15e 258 for(int i = 30; i < 40; i++)
mperella 28:06cefc8dd15e 259 {
mperella 28:06cefc8dd15e 260 temp2 = centers_List[i] + temp2;
mperella 28:06cefc8dd15e 261 }
mperella 28:06cefc8dd15e 262
mperella 28:06cefc8dd15e 263 temp = temp/10;
bbergandy 35:e1cfadfe7469 264 temp2 = temp2/10;
mperella 28:06cefc8dd15e 265
bbergandy 35:e1cfadfe7469 266 if(temp < 73 && temp > 53 && temp2 < 73 && temp2 > 53)
mperella 28:06cefc8dd15e 267 {
bbergandy 35:e1cfadfe7469 268 current_left_motor_speed = current_left_motor_speed - .010;
mperella 28:06cefc8dd15e 269 current_right_motor_speed = current_right_motor_speed + .010;
mperella 28:06cefc8dd15e 270 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
mperella 28:06cefc8dd15e 271 }
bbergandy 36:3491a7d8ffb6 272
mperella 28:06cefc8dd15e 273 }
bbergandy 36:3491a7d8ffb6 274 */
mperella 28:06cefc8dd15e 275
bbbobbbieo 38:ecb6da94cb64 276 // turn left
bbergandy 31:0c2b49175036 277 if (black_center_value < BLACK_THRESHOLD) {
bbbobbbieo 15:830209e846d5 278
bbbobbbieo 38:ecb6da94cb64 279
bbergandy 36:3491a7d8ffb6 280 //current_servo_position = float(.01875*black_center_value-(1.2));
bbbobbbieo 38:ecb6da94cb64 281 previous_servo_position = current_servo_position;
bbbobbbieo 38:ecb6da94cb64 282 //current_servo_position = float(.01875*black_center_value-(1.2));
bbbobbbieo 38:ecb6da94cb64 283 //current_servo_position = float(.000000762939*(black_center_value-64)*(black_center_value-64)*(black_center_value-64))+(0.0046875*(black_center_value))-.3;
bbbobbbieo 38:ecb6da94cb64 284 current_servo_position = float(.00000190735*(black_center_value-64)*(black_center_value-64)*(black_center_value-64))+(0.0109375*(black_center_value))-.7;
bbbobbbieo 38:ecb6da94cb64 285
bbbobbbieo 38:ecb6da94cb64 286 if (current_servo_position-previous_servo_position>SERVO_CAN_MOVE_IN_ONE_FRAME or current_servo_position-previous_servo_position<-SERVO_CAN_MOVE_IN_ONE_FRAME )
bbbobbbieo 38:ecb6da94cb64 287 {current_servo_position = previous_servo_position;}
bbbobbbieo 38:ecb6da94cb64 288
bbbobbbieo 38:ecb6da94cb64 289 if(current_servo_position <= -SERVO_MAX)
bbbobbbieo 38:ecb6da94cb64 290 current_servo_position = -SERVO_MAX;
bbbobbbieo 14:f43b386b8b5d 291 TFC_SetServo(0, current_servo_position);
mperella 26:3f7ddc7d5bdf 292
mperella 37:9c544f53563d 293
bbergandy 36:3491a7d8ffb6 294 //bool listSame = true;
bbergandy 36:3491a7d8ffb6 295 /*
mperella 29:91bda3a63f17 296 int temp = 0;
mperella 29:91bda3a63f17 297 int temp2 = 0;
mperella 28:06cefc8dd15e 298
mperella 28:06cefc8dd15e 299 for(int i = 20; i < 30; i++)
mperella 28:06cefc8dd15e 300 {
mperella 28:06cefc8dd15e 301 temp = centers_List[i] + temp;
mperella 28:06cefc8dd15e 302 }
mperella 28:06cefc8dd15e 303
mperella 28:06cefc8dd15e 304 for(int i = 30; i < 40; i++)
mperella 28:06cefc8dd15e 305 {
mperella 28:06cefc8dd15e 306 temp2 = centers_List[i] + temp2;
mperella 28:06cefc8dd15e 307 }
mperella 28:06cefc8dd15e 308
mperella 28:06cefc8dd15e 309 temp = temp/10;
bbergandy 35:e1cfadfe7469 310 temp2 = temp2/10;
mperella 28:06cefc8dd15e 311
bbergandy 35:e1cfadfe7469 312 if(temp < 73 && temp > 53 && temp2 < 73 && temp2 > 53)
mperella 28:06cefc8dd15e 313 {
bbergandy 35:e1cfadfe7469 314 current_left_motor_speed = current_left_motor_speed - .010;
mperella 28:06cefc8dd15e 315 current_right_motor_speed = current_right_motor_speed + .010;
mperella 28:06cefc8dd15e 316 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
mperella 28:06cefc8dd15e 317 }
mperella 28:06cefc8dd15e 318
bbergandy 36:3491a7d8ffb6 319 */
mperella 28:06cefc8dd15e 320
mperella 28:06cefc8dd15e 321
mperella 26:3f7ddc7d5bdf 322 /*
mperella 26:3f7ddc7d5bdf 323 if(center_now + 5 < center_past_4 && center_now - 5 > center_past_4)
mperella 26:3f7ddc7d5bdf 324 {
mperella 26:3f7ddc7d5bdf 325 accelList[lastAccessed] = 1;
mperella 26:3f7ddc7d5bdf 326 if(lastAccessed != 2)
mperella 26:3f7ddc7d5bdf 327 {
mperella 26:3f7ddc7d5bdf 328 lastAccessed = lastAccessed + 1;
mperella 26:3f7ddc7d5bdf 329 }
mperella 26:3f7ddc7d5bdf 330 else
mperella 26:3f7ddc7d5bdf 331 {
mperella 26:3f7ddc7d5bdf 332 lastAccessed = 0;
mperella 26:3f7ddc7d5bdf 333 }
mperella 26:3f7ddc7d5bdf 334 }
mperella 26:3f7ddc7d5bdf 335
mperella 26:3f7ddc7d5bdf 336
mperella 26:3f7ddc7d5bdf 337
mperella 26:3f7ddc7d5bdf 338 for(int i = 0; i < 3; i++)
mperella 26:3f7ddc7d5bdf 339 {
mperella 26:3f7ddc7d5bdf 340 while(listSame)
mperella 26:3f7ddc7d5bdf 341 {
mperella 26:3f7ddc7d5bdf 342 if(accelList[i] > 0)
mperella 26:3f7ddc7d5bdf 343 {
mperella 26:3f7ddc7d5bdf 344
mperella 26:3f7ddc7d5bdf 345 }
mperella 26:3f7ddc7d5bdf 346 else
mperella 26:3f7ddc7d5bdf 347 {
mperella 26:3f7ddc7d5bdf 348 listSame = false;
mperella 26:3f7ddc7d5bdf 349 }
mperella 26:3f7ddc7d5bdf 350 }
mperella 26:3f7ddc7d5bdf 351 }
mperella 26:3f7ddc7d5bdf 352
mperella 26:3f7ddc7d5bdf 353 if(listSame)
mperella 26:3f7ddc7d5bdf 354 {
mperella 26:3f7ddc7d5bdf 355 current_left_motor_speed = current_left_motor_speed + .010;
mperella 26:3f7ddc7d5bdf 356 current_right_motor_speed = current_right_motor_speed + .010;
mperella 26:3f7ddc7d5bdf 357 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
mperella 26:3f7ddc7d5bdf 358 }
mperella 26:3f7ddc7d5bdf 359
mperella 26:3f7ddc7d5bdf 360
mperella 26:3f7ddc7d5bdf 361 */
bbbobbbieo 15:830209e846d5 362
bbbobbbieo 16:11ba5d6f42ba 363 //current_left_motor_speed = current_left_motor_speed + float(64-black_center_value)*.0025;
bbbobbbieo 16:11ba5d6f42ba 364 //current_right_motor_speed = current_right_motor_speed + float(64-black_center_value)*.0025;
bbbobbbieo 16:11ba5d6f42ba 365 if (violence_level !=0){
bbbobbbieo 39:4a0803d7575d 366 current_left_motor_speed = current_left_motor_speed + float(float(64-black_center_value)*TURN_BACKWARD_ACCEL);// kinda reverse this...
bbbobbbieo 39:4a0803d7575d 367 current_right_motor_speed = current_right_motor_speed + float(float(64-black_center_value)*TURN_FORWARD_ACCEL);// push more forwards
bbbobbbieo 16:11ba5d6f42ba 368 }
bbbobbbieo 16:11ba5d6f42ba 369
bbbobbbieo 16:11ba5d6f42ba 370 // protection block
bbbobbbieo 16:11ba5d6f42ba 371 if(current_left_motor_speed >= 0.5)
bbbobbbieo 16:11ba5d6f42ba 372 current_left_motor_speed= 0.5;
bbbobbbieo 16:11ba5d6f42ba 373 if(current_right_motor_speed >= 0.5)
bbbobbbieo 16:11ba5d6f42ba 374 current_right_motor_speed= 0.5;
bbbobbbieo 16:11ba5d6f42ba 375 if(current_left_motor_speed <= -0.5)
bbbobbbieo 16:11ba5d6f42ba 376 current_left_motor_speed= -0.5;
bbbobbbieo 16:11ba5d6f42ba 377 if(current_right_motor_speed <= -0.5)
bbbobbbieo 16:11ba5d6f42ba 378 current_right_motor_speed= -0.5;
bbbobbbieo 16:11ba5d6f42ba 379
bbbobbbieo 14:f43b386b8b5d 380 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
bbbobbbieo 14:f43b386b8b5d 381
bbbobbbieo 7:455e7dd338ee 382 }
bbbobbbieo 15:830209e846d5 383 // need to turn right
bbergandy 31:0c2b49175036 384 else if (black_center_value > BLACK_THRESHOLD) {
bbbobbbieo 14:f43b386b8b5d 385
bbbobbbieo 38:ecb6da94cb64 386 //current_servo_position = float(.01875*black_center_value-(1.2));
bbbobbbieo 38:ecb6da94cb64 387 previous_servo_position = current_servo_position;
bbbobbbieo 38:ecb6da94cb64 388 //current_servo_position = float(.01875*black_center_value-(1.2));
bbbobbbieo 38:ecb6da94cb64 389 //current_servo_position = float(.000000762939*(black_center_value-64)*(black_center_value-64)*(black_center_value-64))+(0.0046875*(black_center_value))-.3;
bbbobbbieo 38:ecb6da94cb64 390 current_servo_position = float(.00000190735*(black_center_value-64)*(black_center_value-64)*(black_center_value-64))+(0.0109375*(black_center_value))-.7;
bbbobbbieo 38:ecb6da94cb64 391
bbbobbbieo 38:ecb6da94cb64 392 if (current_servo_position-previous_servo_position>SERVO_CAN_MOVE_IN_ONE_FRAME or current_servo_position-previous_servo_position<-SERVO_CAN_MOVE_IN_ONE_FRAME )
bbbobbbieo 38:ecb6da94cb64 393 {current_servo_position = previous_servo_position;}
bbbobbbieo 38:ecb6da94cb64 394
bbbobbbieo 38:ecb6da94cb64 395 if( current_servo_position >= SERVO_MAX)
bbbobbbieo 38:ecb6da94cb64 396 current_servo_position = SERVO_MAX;
bbbobbbieo 14:f43b386b8b5d 397 TFC_SetServo(0, current_servo_position);
mperella 26:3f7ddc7d5bdf 398
bbergandy 36:3491a7d8ffb6 399 //bool listSame = true;
bbergandy 36:3491a7d8ffb6 400 /*
mperella 29:91bda3a63f17 401 int temp = 0;
mperella 29:91bda3a63f17 402 int temp2 = 0;
mperella 28:06cefc8dd15e 403
mperella 28:06cefc8dd15e 404 for(int i = 20; i < 30; i++)
mperella 28:06cefc8dd15e 405 {
mperella 28:06cefc8dd15e 406 temp = centers_List[i] + temp;
mperella 28:06cefc8dd15e 407 }
mperella 28:06cefc8dd15e 408
mperella 28:06cefc8dd15e 409 for(int i = 30; i < 40; i++)
mperella 28:06cefc8dd15e 410 {
mperella 28:06cefc8dd15e 411 temp2 = centers_List[i] + temp2;
mperella 28:06cefc8dd15e 412 }
mperella 28:06cefc8dd15e 413
mperella 28:06cefc8dd15e 414 temp = temp/10;
bbergandy 35:e1cfadfe7469 415 temp2 = temp2/10;
mperella 28:06cefc8dd15e 416
bbergandy 35:e1cfadfe7469 417 if(temp < 73 && temp > 53 && temp2 < 73 && temp2 > 53)
mperella 28:06cefc8dd15e 418 {
bbergandy 35:e1cfadfe7469 419 current_left_motor_speed = current_left_motor_speed - .010;
mperella 28:06cefc8dd15e 420 current_right_motor_speed = current_right_motor_speed + .010;
mperella 28:06cefc8dd15e 421 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
mperella 28:06cefc8dd15e 422 }
mperella 28:06cefc8dd15e 423
mperella 26:3f7ddc7d5bdf 424 /*
mperella 26:3f7ddc7d5bdf 425 if(center_now + 5 < center_past_4 && center_now - 5 > center_past_4)
mperella 26:3f7ddc7d5bdf 426 {
mperella 26:3f7ddc7d5bdf 427 accelList[lastAccessed] = 1;
mperella 26:3f7ddc7d5bdf 428 if(lastAccessed != 2)
mperella 26:3f7ddc7d5bdf 429 {
mperella 26:3f7ddc7d5bdf 430 lastAccessed = lastAccessed + 1;
mperella 26:3f7ddc7d5bdf 431 }
mperella 26:3f7ddc7d5bdf 432 else
mperella 26:3f7ddc7d5bdf 433 {
mperella 26:3f7ddc7d5bdf 434 lastAccessed = 0;
mperella 26:3f7ddc7d5bdf 435 }
mperella 26:3f7ddc7d5bdf 436 }
mperella 26:3f7ddc7d5bdf 437
mperella 26:3f7ddc7d5bdf 438 for(int i = 0; i < 3; i++)
mperella 26:3f7ddc7d5bdf 439 {
mperella 26:3f7ddc7d5bdf 440 while(listSame)
mperella 26:3f7ddc7d5bdf 441 {
mperella 26:3f7ddc7d5bdf 442 if(accelList[i] > 0)
mperella 26:3f7ddc7d5bdf 443 {
mperella 26:3f7ddc7d5bdf 444
mperella 26:3f7ddc7d5bdf 445 }
mperella 26:3f7ddc7d5bdf 446 else
mperella 26:3f7ddc7d5bdf 447 {
mperella 26:3f7ddc7d5bdf 448 listSame = false;
mperella 26:3f7ddc7d5bdf 449 }
mperella 26:3f7ddc7d5bdf 450 }
mperella 26:3f7ddc7d5bdf 451 }
mperella 26:3f7ddc7d5bdf 452
mperella 26:3f7ddc7d5bdf 453 if(listSame)
mperella 26:3f7ddc7d5bdf 454 {
mperella 26:3f7ddc7d5bdf 455 current_left_motor_speed = current_left_motor_speed + .010;
mperella 26:3f7ddc7d5bdf 456 current_right_motor_speed = current_right_motor_speed + .010;
mperella 26:3f7ddc7d5bdf 457 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
mperella 26:3f7ddc7d5bdf 458 }
mperella 26:3f7ddc7d5bdf 459
mperella 26:3f7ddc7d5bdf 460
mperella 26:3f7ddc7d5bdf 461 */
bbbobbbieo 15:830209e846d5 462
bbbobbbieo 16:11ba5d6f42ba 463 //current_left_motor_speed = current_left_motor_speed - float(black_center_value-64)*.0025;
bbbobbbieo 16:11ba5d6f42ba 464 //current_right_motor_speed = current_right_motor_speed - float(black_center_value-64)*.0025;
bbbobbbieo 16:11ba5d6f42ba 465 if (violence_level !=0){
bbbobbbieo 38:ecb6da94cb64 466 current_left_motor_speed = current_left_motor_speed - (float(black_center_value-64))*TURN_FORWARD_ACCEL;// push more forwards
bbbobbbieo 38:ecb6da94cb64 467 current_right_motor_speed = current_right_motor_speed - (float(black_center_value-64))*TURN_BACKWARD_ACCEL;// kinda reverse this...
bbbobbbieo 16:11ba5d6f42ba 468 }
bbbobbbieo 15:830209e846d5 469
bbbobbbieo 16:11ba5d6f42ba 470 // protection block
bbbobbbieo 16:11ba5d6f42ba 471 if(current_left_motor_speed >= 0.5)
bbbobbbieo 16:11ba5d6f42ba 472 current_left_motor_speed= 0.5;
bbbobbbieo 16:11ba5d6f42ba 473 if(current_right_motor_speed >= 0.5)
bbbobbbieo 16:11ba5d6f42ba 474 current_right_motor_speed= 0.5;
bbbobbbieo 16:11ba5d6f42ba 475 if(current_left_motor_speed <= -0.5)
bbbobbbieo 16:11ba5d6f42ba 476 current_left_motor_speed= -0.5;
bbbobbbieo 16:11ba5d6f42ba 477 if(current_right_motor_speed <= -0.5)
bbbobbbieo 16:11ba5d6f42ba 478 current_right_motor_speed= -0.5;
bbbobbbieo 16:11ba5d6f42ba 479
bbbobbbieo 14:f43b386b8b5d 480 TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
bbbobbbieo 15:830209e846d5 481
bbbobbbieo 14:f43b386b8b5d 482 }
bbbobbbieo 14:f43b386b8b5d 483
bbbobbbieo 15:830209e846d5 484 // clearing values for next image processing round
bbbobbbieo 14:f43b386b8b5d 485 black_value_count = 0;
bbbobbbieo 38:ecb6da94cb64 486 //black_center_value = 0;
bbbobbbieo 14:f43b386b8b5d 487 sum_black = 0;
bbbobbbieo 14:f43b386b8b5d 488
bbbobbbieo 15:830209e846d5 489 // end image processing
bbbobbbieo 14:f43b386b8b5d 490
bbbobbbieo 14:f43b386b8b5d 491 linescan_ping_pong = false;
bbbobbbieo 14:f43b386b8b5d 492 } // end checking channel 0
bbbobbbieo 15:830209e846d5 493
bbbobbbieo 15:830209e846d5 494 else { //checking channel 1
bbbobbbieo 14:f43b386b8b5d 495 linescan_ping_pong = true;
bbbobbbieo 14:f43b386b8b5d 496 }
bbbobbbieo 14:f43b386b8b5d 497
bbergandy 25:1d0f586aaf0c 498 TFC_LineScanImageReady = 0; // since we used it, we reset the flag
bbbobbbieo 14:f43b386b8b5d 499 }// end imageready
bbbobbbieo 14:f43b386b8b5d 500 }// end linescan stuff
bbbobbbieo 0:d57117b2188d 501 }
bbbobbbieo 0:d57117b2188d 502 }
bbbobbbieo 8:946806df7347 503
bbbobbbieo 8:946806df7347 504
bbbobbbieo 15:830209e846d5 505 // shit code down here