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.
main.cpp
- Committer:
- mperella
- Date:
- 2015-03-03
- Revision:
- 28:06cefc8dd15e
- Parent:
- 27:3dd1460365d3
- Child:
- 29:91bda3a63f17
File content as of revision 28:06cefc8dd15e:
//#include "mbed.h"
#include "TFC.h"
#define AGGRESSIVE .55
#define MODERATE .48
#define CONSERVATIVE .35
#define STOP 0
#define BLACK_THRESHOLD 64
DigitalOut myled(LED1);
int main()
{
//run this before anything
TFC_Init();
//variables
float current_servo_position = 0;
float current_left_motor_speed = 0;
float current_right_motor_speed = 0;
float dt = 0.00001;
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[128];
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 set_point = 63;
int previous_error = 0;
int error = 0;
for(int i = 0; i < 50; i++)
centers_List[i] = 63;
//uint16_t MyImage0Buffer[2][128];
//uint16_t MyImage1Buffer[2][128];
// 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;
//current_left_motor_speed = (TFC_ReadPot(0));
//current_right_motor_speed = (TFC_ReadPot(1));
// checking behavior level
violence_level = int(TFC_GetDIP_Switch());
if (violence_level==3) {
current_left_motor_speed = -(AGGRESSIVE);
current_right_motor_speed = AGGRESSIVE;
}
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 >= 0.5)
current_left_motor_speed= 0.5;
if(current_right_motor_speed >= 0.5)
current_right_motor_speed= 0.5;
if(current_left_motor_speed <= -0.5)
current_left_motor_speed= -0.5;
if(current_right_motor_speed <= -0.5)
current_right_motor_speed= -0.5;
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;
if (*(TFC_LineScanImage0+64) > 800)
shitnum = 15;
else if((*(TFC_LineScanImage0+64) > 450))
shitnum = 7;
else if((*(TFC_LineScanImage0+64) > 400))
shitnum = 3;
else
shitnum = 1;
TFC_SetBatteryLED(shitnum);
// checking for center line (single line)
for (uint16_t i=0; i<128; i++) {
if ((*(TFC_LineScanImage0+i) < 300)) {
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;
for(int i = 49; i >= 0; i--)
{
if(i = 0)
{
centers_List[i] = center_now;
}
else
{
centers_List[i] = centers_List[i-1];
}
}
// value of center of black (single line)
//black_center_value = sum_black / black_value_count;
center_now = sum_black / black_value_count;
// 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 = (5*center_now + 10*center_past_1 + 15*center_past_2 +30*center_past_3 +40*center_past_4)/100;
/* ******* PID ALGORITHM *******
error = set_point - black_center_value;
integral = integral + error*dt;
derivative = (error - previous_error)/dt;
output = kp*error + ki*integral + kd*derivative;
previous error = error;
GOTTA DO SOME TESTS/SIMULATIONS TO CALIBRATE SERVO ADJUSTMENTS
*****************************
*/
// need to turn left
if (black_center_value == BLACK_THRESHOLD)
{
TFC_SetServo(0,0);
temp = 0;
temp2 = 0;
for(int i = 20; i < 30; i++)
{
temp = centers_List[i] + temp;
}
for(int i = 30; i < 40; i++)
{
temp2 = centers_List[i] + temp2;
}
temp = temp/10;
temp2 = temp/10;
if(temp < 70 && temp > 60 && temp2 < 70 && temp2 > 60)
{
current_left_motor_speed = current_left_motor_speed + .010;
current_right_motor_speed = current_right_motor_speed + .010;
TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
}
}
if (black_center_value < 64) {
current_servo_position= float(.01875*black_center_value-(1.2));
if(current_servo_position <= -0.4)
current_servo_position = -0.4;
TFC_SetServo(0, current_servo_position);
boolean listSame = true;
temp = 0;
temp2 = 0;
for(int i = 20; i < 30; i++)
{
temp = centers_List[i] + temp;
}
for(int i = 30; i < 40; i++)
{
temp2 = centers_List[i] + temp2;
}
temp = temp/10;
temp2 = temp/10;
if(temp < 70 && temp > 60 && temp2 < 70 && temp2 > 60)
{
current_left_motor_speed = current_left_motor_speed + .010;
current_right_motor_speed = current_right_motor_speed + .010;
TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
}
/*
if(center_now + 5 < center_past_4 && center_now - 5 > center_past_4)
{
accelList[lastAccessed] = 1;
if(lastAccessed != 2)
{
lastAccessed = lastAccessed + 1;
}
else
{
lastAccessed = 0;
}
}
for(int i = 0; i < 3; i++)
{
while(listSame)
{
if(accelList[i] > 0)
{
}
else
{
listSame = false;
}
}
}
if(listSame)
{
current_left_motor_speed = current_left_motor_speed + .010;
current_right_motor_speed = current_right_motor_speed + .010;
TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
}
*/
//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(float(64-black_center_value)*.025);// kinda reverse this...
current_right_motor_speed = current_right_motor_speed + float(float(64-black_center_value)*.045);// push more forwards
}
// protection block
if(current_left_motor_speed >= 0.5)
current_left_motor_speed= 0.5;
if(current_right_motor_speed >= 0.5)
current_right_motor_speed= 0.5;
if(current_left_motor_speed <= -0.5)
current_left_motor_speed= -0.5;
if(current_right_motor_speed <= -0.5)
current_right_motor_speed= -0.5;
TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
}
// need to turn right
if (black_center_value > BLACK_THRESHOLD) {
current_servo_position= float(.01875*black_center_value-(1.2));
if( current_servo_position >= +0.4)
current_servo_position = +0.4;
TFC_SetServo(0, current_servo_position);
boolean listSame = true;
temp = 0;
temp2 = 0;
for(int i = 20; i < 30; i++)
{
temp = centers_List[i] + temp;
}
for(int i = 30; i < 40; i++)
{
temp2 = centers_List[i] + temp2;
}
temp = temp/10;
temp2 = temp/10;
if(temp < 70 && temp > 60 && temp2 < 70 && temp2 > 60)
{
current_left_motor_speed = current_left_motor_speed + .010;
current_right_motor_speed = current_right_motor_speed + .010;
TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
}
/*
if(center_now + 5 < center_past_4 && center_now - 5 > center_past_4)
{
accelList[lastAccessed] = 1;
if(lastAccessed != 2)
{
lastAccessed = lastAccessed + 1;
}
else
{
lastAccessed = 0;
}
}
for(int i = 0; i < 3; i++)
{
while(listSame)
{
if(accelList[i] > 0)
{
}
else
{
listSame = false;
}
}
}
if(listSame)
{
current_left_motor_speed = current_left_motor_speed + .010;
current_right_motor_speed = current_right_motor_speed + .010;
TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
}
*/
//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(float(black_center_value-64)*.045);// push more forwards
current_right_motor_speed = current_right_motor_speed - float(float(black_center_value-64)*.025);// kinda reverse this...
}
// protection block
if(current_left_motor_speed >= 0.5)
current_left_motor_speed= 0.5;
if(current_right_motor_speed >= 0.5)
current_right_motor_speed= 0.5;
if(current_left_motor_speed <= -0.5)
current_left_motor_speed= -0.5;
if(current_right_motor_speed <= -0.5)
current_right_motor_speed= -0.5;
TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
}
// 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