fatboyslim / Mbed 2 deprecated bouncesinglecam

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@29:4bf08c74f792, 2015-03-28 (annotated)

Committer:
bbbobbbieo
Date:
Sat Mar 28 22:04:46 2015 +0000
Revision:
29:4bf08c74f792
Parent:
28:add6c7e2f0d3
Child:
30:8e0ad64703d0
runs track, stops. pretty quick. could use in turn acceleration.

Who changed what in which revision?

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