Britney Dorval
Code for robot
Fork of
Yusheng-final_project
by 
Britney Dorval
Diff: main.cpp
- Revision:
- 11:dc410a980771
- Parent:
- 10:5ef5fe8c7775
--- 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
+