Britney Dorval
Code for robot
Fork of
Yusheng-final_project
by 
Britney Dorval
Revision 11:dc410a980771, committed 2017-04-26
- Comitter:
- britneyd
- Date:
- Wed Apr 26 20:58:31 2017 +0000
- Parent:
- 10:5ef5fe8c7775
- Commit message:
- Code for robot;
Changed in this revision
diff -r 5ef5fe8c7775 -r dc410a980771 communication.h
--- a/communication.h Sun Apr 16 19:52:52 2017 +0000
+++ b/communication.h Wed Apr 26 20:58:31 2017 +0000
@@ -42,11 +42,13 @@
// to reject data that doesn't have the right header. So the first
// 8 bytes in txBuffer look like a valid header. The remaining 120
// bytes can be used for anything you like.
-char txBuffer[128];
-char rxBuffer[128];
+#define RF_BUFLEN 200
+char txBuffer[RF_BUFLEN];
+char rxBuffer[RF_BUFLEN];
int rxLen;
+
//***************** Do not change these methods (please) *****************//
/**
* Receive data from the MRF24J40.
@@ -66,6 +68,7 @@
//Make sure our header is valid first
if(data[i] != header[i]) {
//mrf.Reset();
+ //pc.printf("Header not valid\r\n");
return 0;
}
} else {
@@ -74,7 +77,12 @@
}
//pc.printf("Received: %s length:%d\r\n", data, ((int)len)-10);
}
- return ((int)len)-10;
+ int retVal = (int)len-10;
+ if (retVal > 0) {
+ //pc.printf("In rf_receive: got %s\n", data);
+ //pc.printf("Received %d bytes of data\r\n", retVal);
+ }
+ return retVal;
}
/**
@@ -118,6 +126,7 @@
//Pulls data out fo rxBuffer and updates global variables accordingly
void communication_protocal(int len)
{
+ //pc.printf("Found MATCH\r\n");
bool found_name = false;
bool found_num = false;
bool complete_name = false;
@@ -167,7 +176,7 @@
//If we have a complete name AND number value (ie. start and end of each != NONE)
if(found_name & found_num & complete_name & complete_num) {
- //pc.printf("Found MATCH\r\n");
+
//Reset flags
found_name = false;
found_num = false;
@@ -201,12 +210,35 @@
knob4 = (uint8_t)atoi(*num);
else if(strcmp(*name, "Button\0")==0)
button = (bool)atoi(*num);
-
+ else if(strcmp(*name, "Theta\0")==0) {
+ theta_ReadIn = (float)atof(*num);
+ pc.printf("Theta read in: %f\r\n", theta_ReadIn);
+ }
+ else if(strcmp(*name, "Distance\0")==0){
+ dis_ReadIn = (float)atof(*num);
+ pc.printf("Dis read in: %f\r\n", dis_ReadIn);
+ }
//Reset flags
+ /*if(index == 0){
+ waypoints[2*index] = theta_ReadIn;
+ waypoints[2*index+1] = dis_ReadIn;
+ pc.printf("Theta read in: %f\n", theta_ReadIn);
+ pc.printf("Dis read in: %f\n", dis_ReadIn);
+ }
+ else if((theta_ReadIn==0)&&(dis_ReadIn==0)){
+ moving = 1;
+ }
+ else if((waypoints[2*index-2]!= theta_ReadIn)&&(waypoints[2*index-1]!= dis_ReadIn)){
+ waypoints[2*index] = theta_ReadIn;
+ waypoints[2*index+1] = dis_ReadIn;
+ pc.printf("Theta read in: %f\n", theta_ReadIn);
+ pc.printf("Dis read in: %f\n", dis_ReadIn);
+ }*/
name_start = NONE;
name_end = NONE;
num_start = NONE;
num_end = NONE;
+ //index++;
}
}
}
\ No newline at end of file
diff -r 5ef5fe8c7775 -r dc410a980771 main.cpp
--- a/main.cpp Sun Apr 16 19:52:52 2017 +0000
+++ b/main.cpp Wed Apr 26 20:58:31 2017 +0000
@@ -23,7 +23,7 @@
#define NUM_STEPS 1600 //number of steps for 1 turn of wheel
#define WHEEL_RADIUS 51 //units in mm
#define PI 3.1415
-#define maxAccel 5
+#define maxAccel 10
//#define CIRC 2*PI*WHEEL_RADIUS
//For RF Communication
@@ -49,6 +49,8 @@
#define X 500
#define Y 0
+#define COMMUNICATION_FORMAT "Jstick_h: %f Jstick_v: %f Button: %d Theta: %f Distance: %f"
+
//PID
#define MAX_THROTTLE 100
@@ -58,16 +60,22 @@
float Kd1;
float Kp2;
float Kd2;
-int i;
+float theta_ReadIn;
+float dis_ReadIn;
+int i = 0;
+const int waypointBufferSize = 100;
+float waypoints[waypointBufferSize];
+int index = 0;
+int buffer_index = 0;
//Controller Values
uint8_t knob1, knob2, knob3, knob4;
-int8_t jstick_h, jstick_v;
+float jstick_h, jstick_v;
//Control Variables
float throttle = 0; //From joystick
float steering = 0; //From joystick
-int robot_pos = 0; //Robots position
+float robot_pos = 0;//Robots position
float motor1_old = 0;
Timer timer;
@@ -89,7 +97,8 @@
DigitalOut led4(LED4);
//Button
-bool button;
+int button;
+int moving;
#include "communication.h"
//Waypoint
@@ -140,6 +149,7 @@
enable = ENABLE;
while(1) {
+ //pc.printf("Something happened!\r\n");
//Led 4 to indicate if robot is Running or Not
led4 = ROBOT_ON;
@@ -147,15 +157,17 @@
led2 = button;
//If button is pressed reset motor position
+
if(button) {
+ //button = 0;
pos_M1 = 0; //Reset position of Motor 1
pos_M2 = 0; //Reset position of motor 2
//Read Button Press to Toggle Motors
if((timer.read_us() - last_button_time) > 250000) {
ROBOT_ON = ROBOT_ON^1;
- pc.printf("BUTTON WAS PRESSED \r\n");
- last_button_time = timer.read_us();
+ //pc.printf("BUTTON WAS PRESSED \r\n");
+ last_button_time = timer.read_us();
}
}
@@ -174,9 +186,11 @@
//Pose(x,y,th) = P(world_x, world_y, robot_th)
- pc.printf("X axis: %d",world_x);
- pc.printf("Y axis: %d \n",world_y);
- pc.printf("robot_pos: %d \n",robot_pos);
+ //pc.printf("X axis: %d",world_x);
+ //pc.printf("Y axis: %d \n",world_y);
+ //pc.printf("robot_pos: %d \n",robot_pos);
+ //pc.printf("robot_theta: %f\n", robot_theta);
+
//Timer
timer_value = timer.read_us();
@@ -184,25 +198,25 @@
throttle = jstick_v;
steering = jstick_h;
- /**** Update Values DO NOT MODIFY ********/
+ // Update Values DO NOT MODIFY
loop_counter++;
slow_loop_counter++;
medium_loop_counter++;
dt = (timer_value - timer_old);
timer_old = timer_value;
robot_pos_old = robot_pos;
- /*****************************************/
+
//Running: Motor Control Enabled
if(ROBOT_ON) {
+
//TEMPWAYPOINTS
//Get Waypoint
- //i = 0;
- //X = WAYPOINS[i][0]
- //Y = WAYPOINS[i][1]
//calc target theta
- float target_theta = 0;
- float target_distance = 5000;
+ float target_theta = theta_ReadIn*PI/180;//waypoints[2*i];
+ float target_distance = dis_ReadIn;//waypoints[2*i+1];
+ pc.printf("Target_theta = %f\r\n", target_theta);
+ pc.printf("Target_distance = %f\r\n", target_distance);
//if(robot is at waypoint)
//get new waypoint
//i++
@@ -216,74 +230,127 @@
//User Control ONLY
float theta_error = target_theta - robot_theta;
+ float dist_error = target_distance - robot_pos;
+ const float THETA_ERROR_THRESHOLD = 0.1;
+ const float DIS_ERROR_THRESHOLD = 1.0;
+ const float MINIMUM_TURNING_SPEED = 1.0;
+ const float MINIMUM_MOVING_SPEED = 1.0;
- if(theta_error > 1 && theta_error < -1) {
+ if(theta_error > THETA_ERROR_THRESHOLD || theta_error < -THETA_ERROR_THRESHOLD) {
//Angular Controller
- float turn_speed = theta_error*0.5;
+ //pc.printf("theta_error: %f\n", theta_error);
+ float turn_speed = theta_error*5.0;
+ if(turn_speed < MINIMUM_TURNING_SPEED && theta_error > 0){
+ turn_speed = MINIMUM_TURNING_SPEED;
+ }
+ else if(turn_speed > -MINIMUM_TURNING_SPEED && theta_error < 0){
+ turn_speed = -MINIMUM_TURNING_SPEED;
+ }
motor1 = turn_speed;
motor2 = -turn_speed;
- } else {
+ } else if(dist_error > DIS_ERROR_THRESHOLD || dist_error < -DIS_ERROR_THRESHOLD){
//Dist Controller
- float dist_error = target_distance - robot_pos;
+
+ //pc.printf("dist_error: %d\n",dist_error);
float speed = dist_error*0.5;
+ if(speed < MINIMUM_MOVING_SPEED && dist_error > 0){
+ speed = MINIMUM_MOVING_SPEED;
+ }
+ else if(speed > -MINIMUM_TURNING_SPEED && dist_error < 0){
+ speed = -MINIMUM_MOVING_SPEED;
+ }
motor1 = speed;
motor2 = speed;
- }
-
-
- //Acceleratoin Cap
- float motor1Acel = motor1 - motor1_old;
-
- if(motor1Acel > maxAccel) { //macAcel = 5
- motor1 = maxAccel;
- }
+
+
+
+ //Acceleratoin Cap
+ float motor1Acel = motor1 - motor1_old;
+
+ if(motor1Acel > maxAccel) { //macAcel = 5
+ motor1 = maxAccel;
+ motor2 = maxAccel;
+ }
+ else if((motor1<0)&&(motor1Acel<-maxAccel)){
+ motor1 = -maxAccel;
+ motor2 = -maxAccel;
+ }
+
+ //Cap the max and min values [-100, 100]
+ motor1 = CAP(motor1, MAX_CONTROL_OUTPUT);
+ motor2 = CAP(motor2, MAX_CONTROL_OUTPUT);
+ //motor1 = -50;
+ //motor2 = -50;
+ motor1_old = motor1;
+ }else{
+ motor1 = 0.0;
+ motor2 = 0.0;
+ robot_pos = 0.0;
+ }
- //Cap the max and min values [-100, 100]
- motor1 = CAP(motor1, MAX_CONTROL_OUTPUT);
- motor2 = CAP(motor2, MAX_CONTROL_OUTPUT);
- motor1_old = motor1;
//Set Motor Speed here
- setMotor1Speed(motor1);
- setMotor2Speed(motor2);
+ setMotor1Speed(-motor1);
+ setMotor2Speed(-motor2);
}
//Robot is off so disable the motors
else {
enable = DISABLE;
}
+
+
- /* Here are some loops that trigger at different intervals, this
- * will allow you to do things at a slower rate, thus saving speed
- * it is important to keep this fast so we dont miss IMU readings */
+ //Here are some loops that trigger at different intervals, this
+ //will allow you to do things at a slower rate, thus saving speed
+ //it is important to keep this fast so we dont miss IMU readings
//Fast Loop: Good for printing to serial monitor
if(loop_counter >= 5) {
loop_counter = 0;
- pc.printf("ROBOT_ON:%d pos_M1:%d pos_M2:%d robot_pos%d \r\n", ROBOT_ON, pos_M1, pos_M2, robot_pos);
+ //pc.printf("ROBOT_ON:%d pos_M1:%d pos_M2:%d robot_pos%d \r\n", ROBOT_ON, pos_M1, pos_M2, robot_pos);
}
+
+
//Meduim Loop: Good for sending and receiving
if (medium_loop_counter >= 10) {
medium_loop_counter = 0; // Read status
//Recieve Data
- rxLen = rf_receive(rxBuffer, 128);
+ rxLen = rf_receive(rxBuffer, RF_BUFLEN);
+
+ //pc.printf("Tried to receive via rf_receive, rxLen was %d\r\n", rxLen);
if(rxLen > 0) {
led1 = led1^1;
//Process data with our protocal
- communication_protocal(rxLen);
+ //pc.printf("There we go\r\n");
+ //pc.printf("Got %s in rxBuffer\r\n", rxBuffer);
+ int numParsed = sscanf(rxBuffer, COMMUNICATION_FORMAT, &jstick_h, &jstick_v, &button, &theta_ReadIn, &dis_ReadIn);
+ if (numParsed != 5) {
+ // pc.printf("Couldn't parse all parameters. Parsed only %d\r\n", numParsed);
+ } else {
+ /*pc.printf("Jstick_h = %f\r\n", jstick_h);
+ pc.printf("Jstick_v = %f\r\n", jstick_v);
+ pc.printf("button = %d\r\n", button);
+ pc.printf("theta = %f\r\n", theta_ReadIn);
+ pc.printf("distance = %f\r\n", dis_ReadIn);*/
+ }
+ //if((button==1)&&(moving==0)){
+ communication_protocal(rxLen);
+ //}
}
} // End of medium loop
+
//Slow Loop: Good for sending if speed is not an issue
if(slow_loop_counter >= 99) {
slow_loop_counter = 0;
-
- /* Send Data To Controller goes here *
- * */
+ //Send Data To Controller goes here
+
} //End of Slow Loop
+
diff -r 5ef5fe8c7775 -r dc410a980771 stepper_motors.h
--- a/stepper_motors.h Sun Apr 16 19:52:52 2017 +0000
+++ b/stepper_motors.h Wed Apr 26 20:58:31 2017 +0000
@@ -14,13 +14,13 @@
DigitalOut step_M1(MOTOR1_STEP);
DigitalOut dir_M1(MOTOR1_DIR);
DigitalOut enable(MOTOR_ENABLE); //enable for both motors
-int16_t speed_M1; //Speed of motor 1
+float speed_M1; //Speed of motor 1
//MOTOR 2
DigitalOut step_M2(MOTOR2_STEP);
DigitalOut dir_M2(MOTOR2_DIR);
-int16_t speed_M2; //Speed of motor 2
-int16_t motor1, motor2;
+float speed_M2; //Speed of motor 2
+float motor1, motor2;
//Motor Position
int pos_M1 = 0, pos_M2 = 0;
@@ -31,13 +31,14 @@
//ISR to step motor 1
void ISR1(void)
{
+ if (enable == DISABLE) return;
//Step Motor
- step_M1 = 1;
+ step_M1 = 1.0;
wait_us(1);
- step_M1 = 0;
+ step_M1 = 0.0;
//Update Motor Postion
- if(dir_M1)
+ if(!dir_M1)
pos_M1++;
else
pos_M1--;
@@ -45,26 +46,27 @@
//ISR to step motor 2
void ISR2(void)
{
+ if (enable == DISABLE) return;
//Step Motor
- step_M2 = 1;
+ step_M2 = 1.0;
wait_us(1);
- step_M2 = 0;
+ step_M2 = 0.0;
//Update Motor Position
- if(dir_M2)
+ if(!dir_M2)
pos_M2++;
else
pos_M2--;
}
//Set motor 1 speed. Speed [-100, 0, +100] = [Max Reverse, Stop, Max Forward]
-void setMotor1Speed(int16_t speed)
+void setMotor1Speed(float speed)
{
long timer_period;
speed = CAP(speed, MAX_CONTROL_OUTPUT);
//Calculate acceleration from the desired speed
- int16_t desired_accel = speed_M1 - speed;
+ float desired_accel = speed_M1 - speed;
if(desired_accel > MAX_ACCEL)
speed_M1 -= MAX_ACCEL; //Change speed of motor by max acceleration
else if(desired_accel < -MAX_ACCEL)
@@ -72,10 +74,10 @@
else
speed_M1 = speed;
- if(speed_M1 == 0) {
+ if(speed_M1 == 0.0) {
timer_period = ZERO_SPEED;
dir_M1 = 0; ////sets motor direction
- } else if (speed_M1 > 0) {
+ } else if (speed_M1 > 0.0) {
timer_period = 10000 / speed_M1;
dir_M1 = 1; //sets motor direction
} else {
@@ -88,13 +90,13 @@
}
//Set motor 2 speed. Speed [-100, 0, +100] = [Max Reverse, Stop, Max Forward]
-void setMotor2Speed(int16_t speed)
+void setMotor2Speed(float speed)
{
long timer_period;
speed = CAP(speed, MAX_CONTROL_OUTPUT);
//Calculate acceleration from the desired speed
- int16_t desired_accel = speed_M2 - speed;
+ float desired_accel = speed_M2 - speed;
if(desired_accel > MAX_ACCEL)
speed_M2 -= MAX_ACCEL; //Change speed of motor by max acceleration
else if(desired_accel < -MAX_ACCEL)
@@ -102,10 +104,10 @@
else
speed_M2 = speed;
- if(speed_M2 == 0) {
+ if(speed_M2 == 0.0) {
timer_period = ZERO_SPEED;
dir_M2 = 0; ////sets motor direction
- } else if (speed_M2 > 0) {
+ } else if (speed_M2 > 0.0) {
timer_period = 10000 / speed_M2;
dir_M2 = 1; //sets motor direction
} else {