fatboyslim
/
buttontest
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:
- 38:ecb6da94cb64
- Parent:
- 37:9c544f53563d
- Child:
- 39:4a0803d7575d
--- a/main.cpp Wed Mar 04 22:34:12 2015 +0000
+++ b/main.cpp Thu Mar 05 20:03:48 2015 +0000
@@ -2,7 +2,7 @@
#include "TFC.h"
#include <iostream>
#include <stdio.h>
-#include "serialib.h"
+//#include "serialib.h"
#define AGGRESSIVE .55
#define MODERATE .48
@@ -13,7 +13,9 @@
#define PROTECTION_THRESHOLD_UPPER .7
#define PROTECTION_THRESHOLD_LOWER -.7
#define TURN_FORWARD_ACCEL 0.6
-#define TURN_BACKWARD_ACCEL 0.4
+#define TURN_BACKWARD_ACCEL 1.2
+#define SERVO_CAN_MOVE_IN_ONE_FRAME 0.1
+#define SERVO_MAX .5
DigitalOut myled(LED1);
@@ -24,6 +26,7 @@
//variables
float current_servo_position = 0;
+ float previous_servo_position = 0;
float current_left_motor_speed = 0;
float current_right_motor_speed = 0;
@@ -166,7 +169,7 @@
// checking for center line (single line)
for (uint16_t i=0; i<128; i++) {
- if ((*(TFC_LineScanImage0+i) < 300)) {
+ if ((*(TFC_LineScanImage0+i) < 250)) {
black_values_list[black_value_count] = i;
black_value_count++;
}
@@ -198,6 +201,8 @@
// value of center of black (single line)
//black_center_value = sum_black / black_value_count;
+
+ if (black_value_count>5)
center_now = sum_black / black_value_count;
uint8_t num = 0;
@@ -266,12 +271,21 @@
}
*/
+ // turn left
if (black_center_value < BLACK_THRESHOLD) {
+
//current_servo_position = float(.01875*black_center_value-(1.2));
- current_servo_position = float(.01875*center_now-(1.2));
- if(current_servo_position <= -0.4)
- current_servo_position = -0.4;
+ previous_servo_position = current_servo_position;
+ //current_servo_position = float(.01875*black_center_value-(1.2));
+ //current_servo_position = float(.000000762939*(black_center_value-64)*(black_center_value-64)*(black_center_value-64))+(0.0046875*(black_center_value))-.3;
+ current_servo_position = float(.00000190735*(black_center_value-64)*(black_center_value-64)*(black_center_value-64))+(0.0109375*(black_center_value))-.7;
+
+ if (current_servo_position-previous_servo_position>SERVO_CAN_MOVE_IN_ONE_FRAME or current_servo_position-previous_servo_position<-SERVO_CAN_MOVE_IN_ONE_FRAME )
+ {current_servo_position = previous_servo_position;}
+
+ if(current_servo_position <= -SERVO_MAX)
+ current_servo_position = -SERVO_MAX;
TFC_SetServo(0, current_servo_position);
@@ -347,8 +361,8 @@
//current_left_motor_speed = current_left_motor_speed + float(64-black_center_value)*.0025;
//current_right_motor_speed = current_right_motor_speed + float(64-black_center_value)*.0025;
if (violence_level !=0){
- current_left_motor_speed = current_left_motor_speed + float((63-black_center_value)*TURN_BACKWARD_ACCEL);// kinda reverse this...
- current_right_motor_speed = current_right_motor_speed + float((63-black_center_value)*TURN_FORWARD_ACCEL);// push more forwards
+ current_left_motor_speed = current_left_motor_speed + (float(63-black_center_value))*TURN_BACKWARD_ACCEL;// kinda reverse this...
+ current_right_motor_speed = current_right_motor_speed + (float(63-black_center_value))*TURN_FORWARD_ACCEL;// push more forwards
}
// protection block
@@ -367,10 +381,17 @@
// need to turn right
else if (black_center_value > BLACK_THRESHOLD) {
- //current_servo_position= float(.01875*black_center_value-(1.2));
- current_servo_position= float(.01875*center_now-(1.2));
- if( current_servo_position >= 0.4)
- current_servo_position = 0.4;
+ //current_servo_position = float(.01875*black_center_value-(1.2));
+ previous_servo_position = current_servo_position;
+ //current_servo_position = float(.01875*black_center_value-(1.2));
+ //current_servo_position = float(.000000762939*(black_center_value-64)*(black_center_value-64)*(black_center_value-64))+(0.0046875*(black_center_value))-.3;
+ current_servo_position = float(.00000190735*(black_center_value-64)*(black_center_value-64)*(black_center_value-64))+(0.0109375*(black_center_value))-.7;
+
+ if (current_servo_position-previous_servo_position>SERVO_CAN_MOVE_IN_ONE_FRAME or current_servo_position-previous_servo_position<-SERVO_CAN_MOVE_IN_ONE_FRAME )
+ {current_servo_position = previous_servo_position;}
+
+ if( current_servo_position >= SERVO_MAX)
+ current_servo_position = SERVO_MAX;
TFC_SetServo(0, current_servo_position);
//bool listSame = true;
@@ -412,8 +433,6 @@
}
}
-
-
for(int i = 0; i < 3; i++)
{
while(listSame)
@@ -442,8 +461,8 @@
//current_left_motor_speed = current_left_motor_speed - float(black_center_value-64)*.0025;
//current_right_motor_speed = current_right_motor_speed - float(black_center_value-64)*.0025;
if (violence_level !=0){
- current_left_motor_speed = current_left_motor_speed - float((63-black_center_value)*TURN_BACKWARD_ACCEL);// push more forwards
- current_right_motor_speed = current_right_motor_speed - float((63-black_center_value)*TURN_FORWARD_ACCEL);// kinda reverse this...
+ current_left_motor_speed = current_left_motor_speed - (float(black_center_value-64))*TURN_FORWARD_ACCEL;// push more forwards
+ current_right_motor_speed = current_right_motor_speed - (float(black_center_value-64))*TURN_BACKWARD_ACCEL;// kinda reverse this...
}
// protection block
@@ -462,7 +481,7 @@
// clearing values for next image processing round
black_value_count = 0;
- black_center_value = 0;
+ //black_center_value = 0;
sum_black = 0;
// end image processing