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@40:cdb079fc9d0a, 2015-03-09 (annotated)

Committer:
mperella
Date:
Mon Mar 09 19:27:22 2015 +0000
Revision:
40:cdb079fc9d0a
Parent:
39:4a0803d7575d 
added new file for new PID algorithm. haven't tried it out yet. gonna write a short thing in matlab to see what potential values we'd get

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