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.
Revision 13:8a667757beb3, committed 2015-03-26
- Comitter:
- bbbobbbieo
- Date:
- Thu Mar 26 19:44:24 2015 +0000
- Parent:
- 12:915f22e7d7d9
- Child:
- 14:1a408e13679d
- Commit message:
- fuck damn it archive
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/asof3_25_2015_2200h.txt Thu Mar 26 19:44:24 2015 +0000
@@ -0,0 +1,352 @@
+//#include "mbed.h"
+#include "TFC.h"
+#include <iostream>
+#include <stdio.h>
+//#include "serialib.h"
+
+const float AGGRESSIVE = .55;
+const float MODERATE =.48;
+const float CONSERVATIVE =.39;
+const float STOP =0;
+const float PROTECTION_THRESHOLD_UPPER =.7;
+const float PROTECTION_THRESHOLD_LOWER =-.7;
+const float TURN_FORWARD_ACCEL =0.045;
+const float TURN_BACKWARD_ACCEL =0.025;
+const float SERVO_CAN_MOVE_IN_ONE_FRAME =0.1;
+const float SERVO_MAX =.5;
+
+const int BLACK_THRESHOLD =63;
+const int LINE_SCAN_LENGTH =128;
+
+
+DigitalOut myled(LED1);
+
+int main()
+{
+ //run this before anything
+ TFC_Init();
+
+ //variables
+ float current_servo_position = 0;
+ float previous_servo_position = 0;
+ float current_left_motor_speed = 0;
+ float current_right_motor_speed = 0;
+
+ float proportional = 0;
+ float last_proportional = 0;
+ float integral = 0;
+ float derivative = 0;
+ float output = 0;
+
+ // gains on prop, int, der
+ // subject to change, need to fine tune
+ float kp = 1.8960;
+ float ki = 0.6170;
+ float kd = 1.5590;
+
+ bool rear_motor_enable_flag = true;
+ bool linescan_ping_pong = false;
+ bool linescan_enable = true;
+
+ int black_values_list[LINE_SCAN_LENGTH];
+ int black_value_count = 0;
+ int black_center_value = 0;
+ int sum_black = 0;
+ int violence_level = 0;
+
+ int accelList[3];
+ int lastAccessed = 0;
+
+ int centers_List[50];
+
+ int center_now = 63;
+ int center_past_1 = 63;
+ 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;
+
+ for(int i = 0; i < 50; i++)
+ centers_List[i] = 63;
+
+ float left_counter =0;
+ float right_counter =0;
+ bool turn_left=false;
+ bool turn_right=false;
+
+ float bullshit_offset = .074;
+
+ // 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;
+ }
+ else if (violence_level==2) {
+ current_left_motor_speed = -(MODERATE);
+ current_right_motor_speed = (MODERATE);
+ }
+ else if (violence_level==1) {
+ current_left_motor_speed = -(CONSERVATIVE);
+ current_right_motor_speed = CONSERVATIVE;
+ }
+ else if (violence_level==0) {
+ current_left_motor_speed = STOP;
+ current_right_motor_speed = STOP;
+ }
+ else {
+ current_left_motor_speed = STOP;
+ current_right_motor_speed = STOP;
+ }
+
+
+ // protection block
+ if(current_left_motor_speed >= PROTECTION_THRESHOLD_UPPER)
+ current_left_motor_speed= PROTECTION_THRESHOLD_UPPER;
+ if(current_right_motor_speed >= PROTECTION_THRESHOLD_UPPER)
+ current_right_motor_speed = PROTECTION_THRESHOLD_UPPER;
+ if(current_left_motor_speed <= PROTECTION_THRESHOLD_LOWER)
+ current_left_motor_speed = PROTECTION_THRESHOLD_LOWER;
+ 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;
+ }// end motor disabled
+
+ // camera stuff
+ if (linescan_enable) {
+ if (TFC_LineScanImageReady !=0) {
+
+ if (linescan_ping_pong) {
+ //checking channel 0
+
+ //checking center pixel, displays aprox value on leds
+ uint8_t shitnum = 1;
+
+
+ // checking for center line (single line)
+ 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++) {
+ sum_black += black_values_list[i];
+ }
+
+ //update history
+ center_past_4= center_past_3;
+ center_past_3= center_past_2;
+ 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 > 15 && center_now < 27)
+ num = 1;
+ else if(center_now >= 27 && center_now < 54)
+ num = 2;
+ else if(center_now > 60 && center_now < 70)
+ num = 15;
+ else if(center_now >= 54 && center_now < 81)
+ num = 4;
+ else if(center_now >= 81 && center_now < 113)
+ num = 8;
+
+ else
+ num = 0;
+
+ if (black_value_count<2)
+ num = 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
+ //if (black_center_value > BLACK_THRESHOLD+30) {
+ if (num==8 and right_counter <.2 )
+ {
+ //left_counter += (128-black_center_value);
+ //left_counter+=20;
+
+ /*
+ if(left_counter<-0.38)
+ left_counter=-0.5;
+ else
+ left_counter= -0.4;
+ */
+
+ if (left_counter >-.5)
+ left_counter -=.1;
+
+ turn_left=true;
+ turn_right=false;
+
+ }
+ if (num==4 and right_counter <.2)
+ {
+ //left_counter += (128-black_center_value);
+ //left_counter+=20;
+ left_counter=-0.56;
+ turn_left=true;
+ turn_right=false;
+
+ }
+
+ // need to turn right
+ //else if (black_center_value < BLACK_THRESHOLD-30) {
+ else if (num==1 and left_counter >-.2)
+ {
+ //right_counter += black_center_value;
+
+ //right_counter +=20;
+ /*
+ if (right_counter >.38)
+ right_counter =.5;
+ else
+ right_counter =.4;
+ */
+
+ if (right_counter <.5)
+ right_counter +=.1;
+
+ turn_left=false;
+ turn_right=true;
+
+ }
+ else if (num==2 and left_counter >-.2)
+ {
+ //right_counter += black_center_value;
+ //right_counter +=20;
+ right_counter =.56;
+ turn_left=false;
+ turn_right=true;
+
+ }
+ //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));
+ }
+
+ else
+ {
+ //turn_right=false;
+ //turn_left=false;
+ //TFC_SetServo(0,0.0);
+ }
+
+ //dealwiththeshit
+ if(turn_left)
+ {
+ 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;
+
+ TFC_SetMotorPWM(current_left_motor_speed+(.1*left_counter), current_right_motor_speed+(.3*left_counter));
+ //left_counter -= 1;
+ //if (left_counter < 10)
+ // turn_left =false;
+ }
+
+ if(turn_right)
+ {
+ turn_left =false;
+ //TFC_SetServo(0, ((right_counter)*(.05)));
+ TFC_SetServo(0,right_counter);
+ right_counter -= .01;
+ if (right_counter < (0+ bullshit_offset))
+ turn_right = false;
+
+ TFC_SetMotorPWM(current_left_motor_speed-(.3*right_counter), current_right_motor_speed-(.1*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;
+ } // end checking channel 0
+
+ else { //checking channel 1
+ linescan_ping_pong = true;
+ }
+
+ TFC_LineScanImageReady = 0; // since we used it, we reset the flag
+ }// end imageready
+ }// end linescan stuff
+ }
+}
+
+// shit code down here
--- a/asof3_25_2015_2200hours.txt Thu Mar 26 19:41:04 2015 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,352 +0,0 @@
-//#include "mbed.h"
-#include "TFC.h"
-#include <iostream>
-#include <stdio.h>
-//#include "serialib.h"
-
-const float AGGRESSIVE = .55;
-const float MODERATE =.48;
-const float CONSERVATIVE =.39;
-const float STOP =0;
-const float PROTECTION_THRESHOLD_UPPER =.7;
-const float PROTECTION_THRESHOLD_LOWER =-.7;
-const float TURN_FORWARD_ACCEL =0.045;
-const float TURN_BACKWARD_ACCEL =0.025;
-const float SERVO_CAN_MOVE_IN_ONE_FRAME =0.1;
-const float SERVO_MAX =.5;
-
-const int BLACK_THRESHOLD =63;
-const int LINE_SCAN_LENGTH =128;
-
-
-DigitalOut myled(LED1);
-
-int main()
-{
- //run this before anything
- TFC_Init();
-
- //variables
- float current_servo_position = 0;
- float previous_servo_position = 0;
- float current_left_motor_speed = 0;
- float current_right_motor_speed = 0;
-
- float proportional = 0;
- float last_proportional = 0;
- float integral = 0;
- float derivative = 0;
- float output = 0;
-
- // gains on prop, int, der
- // subject to change, need to fine tune
- float kp = 1.8960;
- float ki = 0.6170;
- float kd = 1.5590;
-
- bool rear_motor_enable_flag = true;
- bool linescan_ping_pong = false;
- bool linescan_enable = true;
-
- int black_values_list[LINE_SCAN_LENGTH];
- int black_value_count = 0;
- int black_center_value = 0;
- int sum_black = 0;
- int violence_level = 0;
-
- int accelList[3];
- int lastAccessed = 0;
-
- int centers_List[50];
-
- int center_now = 63;
- int center_past_1 = 63;
- 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;
-
- for(int i = 0; i < 50; i++)
- centers_List[i] = 63;
-
- float left_counter =0;
- float right_counter =0;
- bool turn_left=false;
- bool turn_right=false;
-
- float bullshit_offset = .074;
-
- // 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;
- }
- else if (violence_level==2) {
- current_left_motor_speed = -(MODERATE);
- current_right_motor_speed = (MODERATE);
- }
- else if (violence_level==1) {
- current_left_motor_speed = -(CONSERVATIVE);
- current_right_motor_speed = CONSERVATIVE;
- }
- else if (violence_level==0) {
- current_left_motor_speed = STOP;
- current_right_motor_speed = STOP;
- }
- else {
- current_left_motor_speed = STOP;
- current_right_motor_speed = STOP;
- }
-
-
- // protection block
- if(current_left_motor_speed >= PROTECTION_THRESHOLD_UPPER)
- current_left_motor_speed= PROTECTION_THRESHOLD_UPPER;
- if(current_right_motor_speed >= PROTECTION_THRESHOLD_UPPER)
- current_right_motor_speed = PROTECTION_THRESHOLD_UPPER;
- if(current_left_motor_speed <= PROTECTION_THRESHOLD_LOWER)
- current_left_motor_speed = PROTECTION_THRESHOLD_LOWER;
- 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;
- }// end motor disabled
-
- // camera stuff
- if (linescan_enable) {
- if (TFC_LineScanImageReady !=0) {
-
- if (linescan_ping_pong) {
- //checking channel 0
-
- //checking center pixel, displays aprox value on leds
- uint8_t shitnum = 1;
-
-
- // checking for center line (single line)
- 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++) {
- sum_black += black_values_list[i];
- }
-
- //update history
- center_past_4= center_past_3;
- center_past_3= center_past_2;
- 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 > 15 && center_now < 27)
- num = 1;
- else if(center_now >= 27 && center_now < 54)
- num = 2;
- else if(center_now > 60 && center_now < 70)
- num = 15;
- else if(center_now >= 54 && center_now < 81)
- num = 4;
- else if(center_now >= 81 && center_now < 113)
- num = 8;
-
- else
- num = 0;
-
- if (black_value_count<2)
- num = 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
- //if (black_center_value > BLACK_THRESHOLD+30) {
- if (num==8 and right_counter <.2 )
- {
- //left_counter += (128-black_center_value);
- //left_counter+=20;
-
- /*
- if(left_counter<-0.38)
- left_counter=-0.5;
- else
- left_counter= -0.4;
- */
-
- if (left_counter >-.5)
- left_counter -=.1;
-
- turn_left=true;
- turn_right=false;
-
- }
- if (num==4 and right_counter <.2)
- {
- //left_counter += (128-black_center_value);
- //left_counter+=20;
- left_counter=-0.56;
- turn_left=true;
- turn_right=false;
-
- }
-
- // need to turn right
- //else if (black_center_value < BLACK_THRESHOLD-30) {
- else if (num==1 and left_counter >-.2)
- {
- //right_counter += black_center_value;
-
- //right_counter +=20;
- /*
- if (right_counter >.38)
- right_counter =.5;
- else
- right_counter =.4;
- */
-
- if (right_counter <.5)
- right_counter +=.1;
-
- turn_left=false;
- turn_right=true;
-
- }
- else if (num==2 and left_counter >-.2)
- {
- //right_counter += black_center_value;
- //right_counter +=20;
- right_counter =.56;
- turn_left=false;
- turn_right=true;
-
- }
- //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));
- }
-
- else
- {
- //turn_right=false;
- //turn_left=false;
- //TFC_SetServo(0,0.0);
- }
-
- //dealwiththeshit
- if(turn_left)
- {
- 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;
-
- TFC_SetMotorPWM(current_left_motor_speed+(.1*left_counter), current_right_motor_speed+(.3*left_counter));
- //left_counter -= 1;
- //if (left_counter < 10)
- // turn_left =false;
- }
-
- if(turn_right)
- {
- turn_left =false;
- //TFC_SetServo(0, ((right_counter)*(.05)));
- TFC_SetServo(0,right_counter);
- right_counter -= .01;
- if (right_counter < (0+ bullshit_offset))
- turn_right = false;
-
- TFC_SetMotorPWM(current_left_motor_speed-(.3*right_counter), current_right_motor_speed-(.1*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;
- } // end checking channel 0
-
- else { //checking channel 1
- linescan_ping_pong = true;
- }
-
- TFC_LineScanImageReady = 0; // since we used it, we reset the flag
- }// end imageready
- }// end linescan stuff
- }
-}
-
-// shit code down here
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/asof3_26_2015_1600h.txt Thu Mar 26 19:44:24 2015 +0000
@@ -0,0 +1,343 @@
+//#include "mbed.h"
+#include "TFC.h"
+#include <iostream>
+#include <stdio.h>
+//#include "serialib.h"
+
+const float AGGRESSIVE = .55;
+const float MODERATE =.45;
+const float CONSERVATIVE =.39;
+const float STOP =0;
+const float PROTECTION_THRESHOLD_UPPER =.7;
+const float PROTECTION_THRESHOLD_LOWER =-.7;
+const float TURN_FORWARD_ACCEL =0.045;
+const float TURN_BACKWARD_ACCEL =0.025;
+const float SERVO_CAN_MOVE_IN_ONE_FRAME =0.1;
+const float SERVO_MAX =.5;
+
+const int BLACK_THRESHOLD =63;
+const int LINE_SCAN_LENGTH =128;
+
+
+DigitalOut myled(LED1);
+
+int main()
+{
+ //run this before anything
+ TFC_Init();
+
+ //variables
+ float current_servo_position = 0;
+ float previous_servo_position = 0;
+ float current_left_motor_speed = 0;
+ float current_right_motor_speed = 0;
+
+ float proportional = 0;
+ float last_proportional = 0;
+ float integral = 0;
+ float derivative = 0;
+ float output = 0;
+
+ // gains on prop, int, der
+ // subject to change, need to fine tune
+ float kp = 1.8960;
+ float ki = 0.6170;
+ float kd = 1.5590;
+
+ bool rear_motor_enable_flag = true;
+ bool linescan_ping_pong = false;
+ bool linescan_enable = true;
+
+ int black_values_list[LINE_SCAN_LENGTH];
+ int black_value_count = 0;
+ int black_center_value = 0;
+ int sum_black = 0;
+ int violence_level = 0;
+
+ int accelList[3];
+ int lastAccessed = 0;
+
+ int centers_List[50];
+
+ int center_now = 63;
+ int center_past_1 = 63;
+ 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;
+
+ float left_counter =0;
+ 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 = .074;
+
+ // major loop
+ while(1) {
+
+ // 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;
+ if(current_right_motor_speed >= PROTECTION_THRESHOLD_UPPER)
+ current_right_motor_speed = PROTECTION_THRESHOLD_UPPER;
+ if(current_left_motor_speed <= PROTECTION_THRESHOLD_LOWER)
+ current_left_motor_speed = PROTECTION_THRESHOLD_LOWER;
+ 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;
+ }// end motor disabled
+
+ // camera stuff
+ if (linescan_enable) {
+ if (TFC_LineScanImageReady !=0) {
+
+ if (linescan_ping_pong) {
+ //checking channel 0
+
+ //checking center pixel, displays aprox value on leds
+ uint8_t shitnum = 1;
+
+
+ // checking for center line (single line)
+ 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++) {
+ sum_black += black_values_list[i];
+ }
+
+ //update history
+ center_past_4= center_past_3;
+ center_past_3= center_past_2;
+ 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 > 15 && center_now < 27)
+ num = 1;
+ else if(center_now >= 27 && center_now < 54)
+ num = 2;
+ else if(center_now > 60 && center_now < 70)
+ num = 15;
+ else if(center_now >= 54 && center_now < 81)
+ num = 4;
+ 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;
+
+ TFC_SetBatteryLED(num);
+
+ // best guess of center based on weighted average of history
+ 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
+ {
+ //turn away a little bit for each frame that is wall
+ if (left_counter >-.4)
+ left_counter -=.05;
+ //left_counter -=.03;
+
+ 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
+ {
+ left_counter=-0.5;
+ turn_left=true;
+ turn_right=false;
+ 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 <.4)
+ right_counter +=.05;
+ //right_counter +=.03;
+
+ turn_left=false;
+ turn_right=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
+ {
+ right_counter =.5;
+ turn_left=false;
+ turn_right=true;
+ need_decel=true;
+ }
+
+ // going straight yesssss
+ else if (turn_right == false and turn_left == false and (violence_level !=0))
+ {
+ TFC_SetServo(0,(0.0+ bullshit_offset));
+ TFC_SetMotorPWM(current_left_motor_speed-(.00005*num_of_straight), current_right_motor_speed+(.00005*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{}
+
+ //dealwiththeshit
+ // set servo and motors according to how much left we need to turn
+ if(turn_left and (violence_level !=0))
+ {
+ num_of_straight = 0; // no longer on a straight
+ num_of_right = 0;
+ num_of_left++;
+ turn_right = false;
+ TFC_SetServo(0,left_counter + bullshit_offset ); // set turning servo
+
+ // normalize to center each frame
+ // left turning is - servo
+ if(left_counter > -.2) // small turn, normalize quickly
+ left_counter += .017;
+ else // hard turn, normalize slowly
+ left_counter += .01;
+
+ // no longer turning boolean
+ if (left_counter > (0+ bullshit_offset))
+ turn_left = false;
+
+ if (need_decel) // need to deal with the decel
+ {
+ TFC_SetMotorPWM(current_left_motor_speed+(.55*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+(.3*left_counter), current_right_motor_speed+(.3*left_counter)+(.00005*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))
+ {
+ num_of_straight = 0; // no longer going straight
+ num_of_right++;
+ num_of_left=0;
+
+ turn_left =false;
+ TFC_SetServo(0,right_counter - bullshit_offset); // set servo
+
+ // 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
+ if (right_counter < (0+ bullshit_offset))
+ turn_right = false;
+
+ if(need_decel)// need to deal with the decel
+ {
+ TFC_SetMotorPWM(current_left_motor_speed+(.55*right_counter), current_right_motor_speed-(.4*right_counter)); // ++left is slowed,--right is slowed
+ need_decel = false;
+ }
+ else // turning speeds
+ TFC_SetMotorPWM(current_left_motor_speed-(.3*right_counter)-(.00005*num_of_right), current_right_motor_speed-(.3*right_counter)); // --left is faster, --right is slowed
+ } // end with turn right
+
+
+ // clearing values for next image processing round
+ black_value_count = 0;
+ sum_black = 0;
+ // end image processing
+
+ linescan_ping_pong = false;
+ } // end checking channel 0
+
+ else { //checking channel 1
+ linescan_ping_pong = true;
+ }
+
+ 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
--- a/main.cpp Thu Mar 26 19:41:04 2015 +0000
+++ b/main.cpp Thu Mar 26 19:44:24 2015 +0000
@@ -119,10 +119,12 @@
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);
}