Cooper Liu / Mbed 2 deprecated ICRSEurobot13

Colour sensors calibrated

Dependencies:   mbed-rtos mbed Servo QEI

Fork of ICRSEurobot13 by Thomas Branch

Processes/Motion/motion.cpp@38:6ecf0d21e492, 2013-04-10 (annotated)

Committer:
rsavitski
Date:
Wed Apr 10 20:44:29 2013 +0000
Revision:
38:6ecf0d21e492
Parent:
37:34f4b38039bb
Child:
39:c9058a401410
rubbish angle facing what

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rsavitski 25:50805ef8c499 1 ////////////////////////////////////////////////////////////////////////////////
rsavitski 25:50805ef8c499 2 // Motion control unit
rsavitski 25:50805ef8c499 3 ////////////////////////////////////////////////////////////////////////////////
rsavitski 25:50805ef8c499 4 // Takes current state of the robot and target waypoint,
rsavitski 25:50805ef8c499 5 // calculates desired forward and angular velocities and requests those from the motor control layer.
rsavitski 25:50805ef8c499 6 ////////////////////////////////////////////////////////////////////////////////
rsavitski 25:50805ef8c499 7
rsavitski 25:50805ef8c499 8 #include "motion.h"
rsavitski 36:f8e7f0a72a3d 9 DigitalOut OLED4(LED4);
rsavitski 38:6ecf0d21e492 10 DigitalOut OLED1(LED1);
rsavitski 25:50805ef8c499 11 namespace motion
rsavitski 25:50805ef8c499 12 {
rsavitski 25:50805ef8c499 13
rsavitski 25:50805ef8c499 14 void motionlayer(void const *dummy)
rsavitski 31:791739422122 15 {
rsavitski 25:50805ef8c499 16 // get target waypoint from AI
rsavitski 25:50805ef8c499 17 Waypoint target_waypoint = *AI::current_waypoint;
rsavitski 25:50805ef8c499 18
rsavitski 25:50805ef8c499 19 // get current state from Kalman
rsavitski 25:50805ef8c499 20 State current_state = Kalman::getState();
rsavitski 25:50805ef8c499 21
rsavitski 25:50805ef8c499 22 float delta_x = target_waypoint.x - current_state.x;
rsavitski 25:50805ef8c499 23 float delta_y = target_waypoint.y - current_state.y;
rsavitski 25:50805ef8c499 24
madcowswe 26:b16f1045108f 25 //printf("motion sees deltax: %f deltay %f\r\n", delta_x, delta_y);
rsavitski 25:50805ef8c499 26
madcowswe 26:b16f1045108f 27 float distance_err = hypot(delta_x, delta_y);
madcowswe 26:b16f1045108f 28
madcowswe 26:b16f1045108f 29 float angle_err = constrainAngle(atan2(delta_y, delta_x) - current_state.theta);
rsavitski 25:50805ef8c499 30
rsavitski 38:6ecf0d21e492 31 bool d_reached = false;
rsavitski 38:6ecf0d21e492 32 bool a_reached = false;
rsavitski 38:6ecf0d21e492 33
rsavitski 31:791739422122 34 // is the waypoint reached
rsavitski 31:791739422122 35 if (distance_err < target_waypoint.pos_threshold)
rsavitski 31:791739422122 36 {
rsavitski 38:6ecf0d21e492 37 d_reached = true;
rsavitski 31:791739422122 38 distance_err = 0;
rsavitski 38:6ecf0d21e492 39 OLED1 = 1;
rsavitski 36:f8e7f0a72a3d 40 if (abs(constrainAngle(target_waypoint.theta - current_state.theta)) < target_waypoint.angle_threshold)
madcowswe 34:a49197572737 41 {
rsavitski 38:6ecf0d21e492 42 a_reached = true;
madcowswe 34:a49197572737 43 angle_err = 0;
madcowswe 34:a49197572737 44 }
rsavitski 36:f8e7f0a72a3d 45 }
rsavitski 31:791739422122 46
rsavitski 31:791739422122 47 AI::waypoint_flag_mutex.lock(); // proper way would be to construct a function to evaluate the condition and pass the function pointer to a conditional setter function for reached flag
rsavitski 38:6ecf0d21e492 48 if (d_reached && a_reached)
rsavitski 31:791739422122 49 {
rsavitski 36:f8e7f0a72a3d 50 OLED4 = 1;
rsavitski 31:791739422122 51 AI::setWaypointReached();
rsavitski 31:791739422122 52 return;
rsavitski 31:791739422122 53 }
rsavitski 31:791739422122 54 AI::waypoint_flag_mutex.unlock();
rsavitski 25:50805ef8c499 55
rsavitski 25:50805ef8c499 56 // angular velocity controller
madcowswe 35:e1678450feec 57 const float p_gain_av = 0.5; //TODO: tune
rsavitski 25:50805ef8c499 58
madcowswe 34:a49197572737 59 const float max_av = 0.5*PI; // radians per sec //TODO: tune
rsavitski 25:50805ef8c499 60
rsavitski 25:50805ef8c499 61 // angle error [-pi, pi]
rsavitski 25:50805ef8c499 62 float angular_v = p_gain_av * angle_err;
rsavitski 25:50805ef8c499 63
rsavitski 25:50805ef8c499 64 // constrain range
rsavitski 25:50805ef8c499 65 if (angular_v > max_av)
rsavitski 25:50805ef8c499 66 angular_v = max_av;
rsavitski 25:50805ef8c499 67 else if (angular_v < -max_av)
rsavitski 25:50805ef8c499 68 angular_v = -max_av;
rsavitski 25:50805ef8c499 69
rsavitski 25:50805ef8c499 70
rsavitski 25:50805ef8c499 71 // forward velocity controller
madcowswe 35:e1678450feec 72 const float p_gain_fv = 0.5; //TODO: tune
rsavitski 25:50805ef8c499 73
madcowswe 26:b16f1045108f 74 float max_fv = 0.2; // meters per sec //TODO: tune
rsavitski 25:50805ef8c499 75 const float angle_envelope_exponent = 8.0; //TODO: tune
rsavitski 25:50805ef8c499 76
rsavitski 25:50805ef8c499 77 // control, distance_err in meters
rsavitski 25:50805ef8c499 78 float forward_v = p_gain_fv * distance_err;
rsavitski 25:50805ef8c499 79
rsavitski 25:50805ef8c499 80 // control the forward velocity envelope based on angular error
rsavitski 25:50805ef8c499 81 max_fv = max_fv * pow(cos(angle_err/2), angle_envelope_exponent);
rsavitski 25:50805ef8c499 82
rsavitski 25:50805ef8c499 83 // constrain range
rsavitski 25:50805ef8c499 84 if (forward_v > max_fv)
rsavitski 25:50805ef8c499 85 forward_v = max_fv;
rsavitski 25:50805ef8c499 86 else if (forward_v < -max_fv)
rsavitski 25:50805ef8c499 87 forward_v = -max_fv;
madcowswe 26:b16f1045108f 88
madcowswe 26:b16f1045108f 89 //printf("fwd: %f, omega: %f\r\n", forward_v, angular_v);
rsavitski 25:50805ef8c499 90
rsavitski 31:791739422122 91 // pass values to the motor control
rsavitski 25:50805ef8c499 92 MotorControl::set_fwdcmd(forward_v);
madcowswe 26:b16f1045108f 93 MotorControl::set_omegacmd(angular_v);
rsavitski 25:50805ef8c499 94 }
rsavitski 25:50805ef8c499 95
rsavitski 25:50805ef8c499 96 } //namespace