fatboyslim / Mbed 2 deprecated bouncesinglecam

Dependencies:   FRDM-TFC mbed

Committer:
bbbobbbieo
Date:
Sun Mar 29 18:00:19 2015 +0000
Revision:
31:55d26f4ef5f0
asof3_29_2015_1400h.txt can run the track, has issues stopping, doesnt see the line. using a pot for black levels

Who changed what in which revision?

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