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 5:e6c21518fa48, committed 2015-03-26
- Comitter:
- bbbobbbieo
- Date:
- Thu Mar 26 02:21:33 2015 +0000
- Parent:
- 4:18f5328ff5a3
- Child:
- 6:ed97e4324202
- Commit message:
- added text files for logging? im about to cut some shit out;
Changed in this revision
| asof3_25_2015_2200hours.txt | Show annotated file Show diff for this revision Revisions of this file |
| shit.txt | Show annotated file Show diff for this revision Revisions of this file |
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/asof3_25_2015_2200hours.txt Thu Mar 26 02:21:33 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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/shit.txt Thu Mar 26 02:21:33 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