fatboyslim
/
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.
Diff: main.cpp
- Revision:
- 25:806e67a1218f
- Parent:
- 23:a141ca857b8f
- Child:
- 27:aa81c15e5c1d
--- a/main.cpp Fri Mar 27 19:18:29 2015 +0000
+++ b/main.cpp Sat Mar 28 20:53:49 2015 +0000
@@ -5,8 +5,8 @@
//#include "serialib.h"
const float AGGRESSIVE = .55;
-const float MODERATE =.45;
-const float CONSERVATIVE =.33;
+const float MODERATE =.48;
+const float CONSERVATIVE =.39;
const float STOP =0;
const float PROTECTION_THRESHOLD_UPPER =.7;
const float PROTECTION_THRESHOLD_LOWER =-.7;
@@ -64,15 +64,12 @@
int center_past_2 = 63;
int center_past_3 = 63;
int center_past_4 = 63;
+ //int best_guess_center = 64;
int position = 0;
int set_point = 63;
int previous_error = 0;
int error = 0;
-
- float increment = 0;
- int right_turn_count = 0;
- int left_turn_count = 0;
for(int i = 0; i < 50; i++)
centers_List[i] = 63;
@@ -81,51 +78,64 @@
float right_counter =0;
bool turn_left=false;
bool turn_right=false;
- bool need_decel=false;
- int num_of_straight =0;
- int num_of_left =0;
- int num_of_right=0;
- //servo is offset zero by some bullshit number
- float bullshit_offset = .068;
+ float bullshit_offset = .08;
// major loop
while(1) {
+ // manual servo control, unused
+ if (TFC_ReadPushButton(0) != 0 ) {
+ current_servo_position = current_servo_position-.005;
+ if(current_servo_position <= -0.4)
+ current_servo_position = -0.4;
+ TFC_SetServo(0, current_servo_position);
+ }// end check button0
+
+ else {}
+
+
+ // manual servo control, unused
+ if (TFC_ReadPushButton(1) != 0 ) {
+ current_servo_position = current_servo_position+.005;
+ if(current_servo_position >= 0.4)
+ current_servo_position = 0.4;
+ TFC_SetServo(0, current_servo_position);
+ }// end check button1
+
+ else {}
+
// initial motor stuff
if(rear_motor_enable_flag) {
TFC_HBRIDGE_ENABLE;
+
// checking behavior level
violence_level = int(TFC_GetDIP_Switch());
if (violence_level==3) {
current_left_motor_speed = -(AGGRESSIVE);
current_right_motor_speed = AGGRESSIVE;
- increment = 0.02;
}
else if (violence_level==2) {
current_left_motor_speed = -(MODERATE);
current_right_motor_speed = (MODERATE);
- increment = 0.03;
}
else if (violence_level==1) {
current_left_motor_speed = -(CONSERVATIVE);
current_right_motor_speed = CONSERVATIVE;
- increment = 0.04;
}
else if (violence_level==0) {
current_left_motor_speed = STOP;
current_right_motor_speed = STOP;
- TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
}
else {
current_left_motor_speed = STOP;
current_right_motor_speed = STOP;
- TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
}
+
// protection block
if(current_left_motor_speed >= PROTECTION_THRESHOLD_UPPER)
current_left_motor_speed= PROTECTION_THRESHOLD_UPPER;
@@ -136,6 +146,7 @@
if(current_right_motor_speed <= PROTECTION_THRESHOLD_LOWER)
current_right_motor_speed = PROTECTION_THRESHOLD_LOWER;
+ TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
}// end motor enabled
else {
TFC_HBRIDGE_DISABLE;
@@ -151,16 +162,18 @@
//checking center pixel, displays aprox value on leds
uint8_t shitnum = 1;
+
// checking for center line (single line)
- for (uint16_t i=10; i<118; i++) {
- if ((*(TFC_LineScanImage0+i) < 425)) {
+ for (uint16_t i=15; i<113; i++) {
+ if ((*(TFC_LineScanImage0+i) < 450)) {
black_values_list[black_value_count] = i;
black_value_count++;
}
}
- for(int i=0; i<black_value_count; i++)
+ for(int i=0; i<black_value_count; i++) {
sum_black += black_values_list[i];
+ }
//update history
center_past_4= center_past_3;
@@ -168,198 +181,147 @@
center_past_2= center_past_1;
center_past_1= center_now;
+
+ //if (black_value_count>2)
center_now = sum_black / black_value_count;
uint8_t num = 0;
- if(center_now > 10 && center_now < 45) // linear func
+ if(center_now > 15 && center_now < 27)
num = 1;
- else if(center_now >= 45 && center_now < 65) // snap
+ else if(center_now >= 27 && center_now < 54)
num = 2;
- else if(center_now > 64 && center_now < 64) // nothing zone
+ else if(center_now > 60 && center_now < 70)
num = 15;
- else if(center_now >= 65 && center_now < 83) // snap
+ else if(center_now >= 54 && center_now < 81)
num = 4;
- else if(center_now >= 83 && center_now < 118) // linear func
+ else if(center_now >= 81 && center_now < 113)
num = 8;
else
num = 0;
- // get rid of garbage data sets
if (black_value_count<2)
num = 0;
-
+
if (black_value_count>30)
{
while(1)
TFC_SetMotorPWM(0, 0);
- }
+ }
+
TFC_SetBatteryLED(num);
// best guess of center based on weighted average of history
+ //black_center_value = (5*center_now + 10*center_past_1 + 15*center_past_2 +30*center_past_3 +40*center_past_4)/100;
+ //black_center_value = (15*center_now + 15*center_past_1 + 15*center_past_2 +25*center_past_3 +30*center_past_4)/100;
black_center_value = center_now;
// turn left
- // hit wall a little bit on the right
- if (num==8 and right_counter <.2 ) // only if we arent turning right
+ //if (black_center_value > BLACK_THRESHOLD+30) {
+ if (num==8 and right_counter <.2 )
{
- //turn away a little bit for each frame that is wall
- if (left_counter >-.45)
- {
- if (center_now>100)
- //left_counter -=.03;
- left_counter -=.027;
- else
- //left_counter -=.05;
- left_counter -=.044;
- }
-
- if (left_counter <-.46)
- {
- num_of_left =0;
- }
-
-
+ //left_counter += (128-black_center_value);
+ //left_counter+=20;
+ left_counter=-0.4;
turn_left=true;
turn_right=false;
+
}
-
- // turn left real hard
- // wall is close to center on right
- if (num==4 and right_counter <.2) // only if we arent turning right
+ if (num==4 and right_counter <.2)
{
- left_counter=-0.54;
+ //left_counter += (128-black_center_value);
+ //left_counter+=20;
+ left_counter=-0.6;
turn_left=true;
turn_right=false;
-
- num_of_left =0;
- need_decel=true;
+
}
- // turn right hard
- // wall is close to center on left
- else if (num==2 and left_counter >-.2)// only if we arent turning left
+ // need to turn right
+ //else if (black_center_value < BLACK_THRESHOLD-30) {
+ else if (num==1 and left_counter >-.2)
{
- right_counter =.54;
+ //right_counter += black_center_value;
+
+ //right_counter +=20;
+ right_counter =.4;
turn_left=false;
turn_right=true;
-
- num_of_right =0;
- need_decel=true;
+
}
-
-
- // turn right
- // hit wall a little bit on the left
- else if (num==1 and left_counter >-.2) // only if we arent turning left
- {
- //turn away a little bit for each frame that is wall
- if (right_counter <.45)
- {
- if (center_now<28)
- right_counter +=.03;
- else
- right_counter +=.05;
- }
-
- if (right_counter >.46)
- {
- num_of_right =0;
- }
-
+ else if (num==2 and left_counter >-.2)
+ {
+ //right_counter += black_center_value;
+ //right_counter +=20;
+ right_counter =.6;
turn_left=false;
turn_right=true;
+
}
-
-
- // going straight yesssss
- else if (turn_right == false and turn_left == false and (violence_level !=0))
+ //else if (black_value_count < 2)
+ // {
+ // turn_right=false;
+ //// turn_left=false;
+ // TFC_SetServo(0,0.0);
+ // }
+
+ //else if (num==0)
+ //{
+ // TFC_SetServo(0,0.0);
+ //}
+
+
+ else if (turn_right == false and turn_left == false)
{
TFC_SetServo(0,(0.0+ bullshit_offset));
- TFC_SetMotorPWM(current_left_motor_speed-(.00008*num_of_straight), current_right_motor_speed+(.00008*num_of_straight)); // --left is faster, ++right is faster
- num_of_left = 0;
- num_of_right= 0;
- if (violence_level !=0)
- num_of_straight++;
}
- else{}
+ else
+ {
+ //turn_right=false;
+ //turn_left=false;
+ //TFC_SetServo(0,0.0);
+ }
//dealwiththeshit
- // set servo and motors according to how much left we need to turn
- if(turn_left and (violence_level !=0))
+ if(turn_left)
{
- num_of_straight = 0; // no longer on a straight
- num_of_right = 0;
- num_of_left++;
- turn_right = false;
-
- if (left_counter + bullshit_offset > -.30)
- TFC_SetServo(0,left_counter + bullshit_offset ); // set turning servo
- else
- TFC_SetServo(0, -.30);
-
- // normalize to center each frame
- // left turning is - servo
- if(left_counter > -.2) // small turn, normalize quickly
- left_counter += .009;
- else // hard turn, normalize slowly
- left_counter += .008;
-
- // no longer turning boolean
+ turn_right = false;
+ //TFC_SetServo(0, ((left_counter)*(-.05)));
+ TFC_SetServo(0,left_counter);
+ left_counter += .01;
if (left_counter > (0+ bullshit_offset))
turn_left = false;
-
- if (need_decel) // need to deal with the decel
- {
- TFC_SetMotorPWM(current_left_motor_speed+(.5*left_counter), current_right_motor_speed-(.4*left_counter)); // ++left is slowed,--right is slowed
- need_decel = false;
- }
- else // turning speeds
- TFC_SetMotorPWM(current_left_motor_speed+(.26*left_counter), current_right_motor_speed+(.36*left_counter)+(.0001*num_of_left)); // ++left is slowed, ++right is faster
- }// end of turn left
-
- // set servo and motors according to how much right we need to turn
- if(turn_right and (violence_level !=0))
+
+ TFC_SetMotorPWM(current_left_motor_speed+(.2*left_counter), current_right_motor_speed+(.2*left_counter));
+ //left_counter -= 1;
+ //if (left_counter < 10)
+ // turn_left =false;
+ }
+
+ if(turn_right)
{
- num_of_straight = 0; // no longer going straight
- num_of_right++;
- num_of_left=0;
turn_left =false;
-
- if (right_counter - bullshit_offset < .5)
- TFC_SetServo(0,right_counter - bullshit_offset); // set servo
- else
- TFC_SetServo(0,.5);
-
- // normalize to center each frame
- // right turning is + servo
- if(right_counter < .2) // small turn, normalize quickly
- right_counter -= .017;
- else // hard turn, normalize slowly
- right_counter -= .01;
-
-
- // no longer turning boolean
+ //TFC_SetServo(0, ((right_counter)*(.05)));
+ TFC_SetServo(0,right_counter);
+ right_counter -= .01;
if (right_counter < (0+ bullshit_offset))
turn_right = false;
-
- if(need_decel)// need to deal with the decel
- {
- TFC_SetMotorPWM(current_left_motor_speed+(.4*right_counter), current_right_motor_speed-(.5*right_counter)); // ++left is slowed,--right is slowed
- need_decel = false;
- }
- else // turning speeds
- TFC_SetMotorPWM(current_left_motor_speed-(.35*right_counter)-(.0001*num_of_right), current_right_motor_speed-(.25*right_counter)); // --left is faster, --right is slowed
- } // end with turn right
-
+
+ TFC_SetMotorPWM(current_left_motor_speed-(.2*right_counter), current_right_motor_speed-(.2*right_counter));
+ //right_counter -= 1;
+ // if (right_counter < 10)
+ // turn_right =false;
+ }
// clearing values for next image processing round
black_value_count = 0;
+ //black_center_value = 0;
sum_black = 0;
+
// end image processing
linescan_ping_pong = false;
@@ -372,5 +334,8 @@
TFC_LineScanImageReady = 0; // since we used it, we reset the flag
}// end imageready
}// end linescan stuff
- } // end major loop
-}// end main
\ No newline at end of file
+ }
+}
+
+// shit code down here
+