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@25:50805ef8c499, 2013-04-09 (annotated)

Committer:
rsavitski
Date:
Tue Apr 09 20:37:59 2013 +0000
Revision:
25:50805ef8c499
Child:
26:b16f1045108f
Motion control branch

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 25:50805ef8c499 9
rsavitski 25:50805ef8c499 10 namespace motion
rsavitski 25:50805ef8c499 11 {
rsavitski 25:50805ef8c499 12
rsavitski 25:50805ef8c499 13 void motionlayer(void const *dummy)
rsavitski 25:50805ef8c499 14 {
rsavitski 25:50805ef8c499 15 //TODO: current_waypoint global in AI layer
rsavitski 25:50805ef8c499 16 //TODO: threshold for deciding that the waypoint has been achieved
rsavitski 25:50805ef8c499 17
rsavitski 25:50805ef8c499 18 // get target waypoint from AI
rsavitski 25:50805ef8c499 19 Waypoint target_waypoint = *AI::current_waypoint;
rsavitski 25:50805ef8c499 20
rsavitski 25:50805ef8c499 21 // get current state from Kalman
rsavitski 25:50805ef8c499 22 State current_state = Kalman::getState();
rsavitski 25:50805ef8c499 23
rsavitski 25:50805ef8c499 24 float delta_x = target_waypoint.x - current_state.x;
rsavitski 25:50805ef8c499 25 float delta_y = target_waypoint.y - current_state.y;
rsavitski 25:50805ef8c499 26
rsavitski 25:50805ef8c499 27 float distance_err = sqrt(delta_x*delta_x + delta_y*delta_y);
rsavitski 25:50805ef8c499 28
rsavitski 25:50805ef8c499 29 float angle_err = atan2(delta_y, delta_x);
rsavitski 25:50805ef8c499 30
rsavitski 25:50805ef8c499 31
rsavitski 25:50805ef8c499 32 // angular velocity controller
rsavitski 25:50805ef8c499 33 const float p_gain_av = 1.0; //TODO: tune
rsavitski 25:50805ef8c499 34
rsavitski 25:50805ef8c499 35 const float max_av = PI; // radians per sec //TODO: tune
rsavitski 25:50805ef8c499 36
rsavitski 25:50805ef8c499 37 // angle error [-pi, pi]
rsavitski 25:50805ef8c499 38 float angular_v = p_gain_av * angle_err;
rsavitski 25:50805ef8c499 39
rsavitski 25:50805ef8c499 40 // constrain range
rsavitski 25:50805ef8c499 41 if (angular_v > max_av)
rsavitski 25:50805ef8c499 42 angular_v = max_av;
rsavitski 25:50805ef8c499 43 else if (angular_v < -max_av)
rsavitski 25:50805ef8c499 44 angular_v = -max_av;
rsavitski 25:50805ef8c499 45
rsavitski 25:50805ef8c499 46
rsavitski 25:50805ef8c499 47 // forward velocity controller
rsavitski 25:50805ef8c499 48 const float p_gain_fv = 1.0; //TODO: tune
rsavitski 25:50805ef8c499 49
rsavitski 25:50805ef8c499 50 float max_fv = 1.0; // meters per sec //TODO: tune
rsavitski 25:50805ef8c499 51 const float angle_envelope_exponent = 8.0; //TODO: tune
rsavitski 25:50805ef8c499 52
rsavitski 25:50805ef8c499 53 // control, distance_err in meters
rsavitski 25:50805ef8c499 54 float forward_v = p_gain_fv * distance_err;
rsavitski 25:50805ef8c499 55
rsavitski 25:50805ef8c499 56 // control the forward velocity envelope based on angular error
rsavitski 25:50805ef8c499 57 max_fv = max_fv * pow(cos(angle_err/2), angle_envelope_exponent);
rsavitski 25:50805ef8c499 58
rsavitski 25:50805ef8c499 59 // constrain range
rsavitski 25:50805ef8c499 60 if (forward_v > max_fv)
rsavitski 25:50805ef8c499 61 forward_v = max_fv;
rsavitski 25:50805ef8c499 62 else if (forward_v < -max_fv)
rsavitski 25:50805ef8c499 63 forward_v = -max_fv;
rsavitski 25:50805ef8c499 64
rsavitski 25:50805ef8c499 65 //TODO: put into motor control
rsavitski 25:50805ef8c499 66 MotorControl::set_fwdcmd(forward_v);
rsavitski 25:50805ef8c499 67 MotorControl::set_thetacmd(angular_v);
rsavitski 25:50805ef8c499 68 }
rsavitski 25:50805ef8c499 69
rsavitski 25:50805ef8c499 70 } //namespace