Whose Line is it Anyways / Mbed OS PM2_WLiiA_robot_code

robot code for summer school

Dependencies:   PM2_Libary Eigen

Fork of PM2_Example_Summer_School by Alex Hawkins

Robot_Library/robot.cpp@54:b442660523df, 2022-05-26 (annotated)

Committer:
eversonrosed
Date:
Thu May 26 14:48:31 2022 +0200
Revision:
54:b442660523df
Parent:
49:7da71f479dac
Child:
57:8bf0b5a70065
dtor

Who changed what in which revision?

UserRevisionLine numberNew contents of line
seas726 49:7da71f479dac 1 #include "robot.h"
seas726 49:7da71f479dac 2 #include "EncoderCounter.h"
seas726 49:7da71f479dac 3 #include "PeripheralNames.h"
seas726 49:7da71f479dac 4 #include "PinNames.h"
seas726 49:7da71f479dac 5 #include <cstdio>
seas726 49:7da71f479dac 6
seas726 49:7da71f479dac 7 Robot::Robot() : dist(PB_1),
seas726 49:7da71f479dac 8 bit0(PH_1),
seas726 49:7da71f479dac 9 bit1(PC_2),
seas726 49:7da71f479dac 10 bit2(PC_3),
seas726 49:7da71f479dac 11 ir_sensor_0(dist, bit0, bit1, bit2, 0), // one IR sendor
seas726 49:7da71f479dac 12 i2c2(PB_9, PB_8),
seas726 49:7da71f479dac 13 line_sensor(i2c2),
seas726 49:7da71f479dac 14 pwm_M1(PA_9),
seas726 49:7da71f479dac 15 pwm_M2(PA_8),
seas726 49:7da71f479dac 16 encoder_M1(PA_6, PC_7),
seas726 49:7da71f479dac 17 encoder_M2(PB_6,PB_7)
seas726 49:7da71f479dac 18 {
seas726 49:7da71f479dac 19 // initialize all variables
seas726 49:7da71f479dac 20 wheel_to_robot << -WHEEL_RADIUS / 2.0f, WHEEL_RADIUS / 2.0f,
seas726 49:7da71f479dac 21 -WHEEL_RADIUS / DISTANCE_BETWEEN_WHEELS, -WHEEL_RADIUS / DISTANCE_BETWEEN_WHEELS; // transformation matri
seas726 49:7da71f479dac 22 robot_to_wheel = wheel_to_robot.inverse();
seas726 49:7da71f479dac 23
seas726 49:7da71f479dac 24 robot_speed_desired.setZero(); // zero out all speed
seas726 49:7da71f479dac 25 wheel_speed_desired.setZero();
seas726 49:7da71f479dac 26 wheel_speed_smooth.setZero();
seas726 49:7da71f479dac 27 robot_speed_actual.setZero();
seas726 49:7da71f479dac 28 wheel_speed_actual.setZero();
seas726 49:7da71f479dac 29
seas726 49:7da71f479dac 30 // MOTORS + MOTION
seas726 49:7da71f479dac 31
seas726 49:7da71f479dac 32 // TRAJECTORY PLANNERS
seas726 49:7da71f479dac 33 trajectoryPlanners[0] = new Motion();
seas726 49:7da71f479dac 34 trajectoryPlanners[1] = new Motion();
seas726 49:7da71f479dac 35
seas726 49:7da71f479dac 36 trajectoryPlanners[0]->setProfileVelocity(MAX_MOTOR_VOLTAGE * KN / 60.0f);
seas726 49:7da71f479dac 37 trajectoryPlanners[1]->setProfileVelocity(MAX_MOTOR_VOLTAGE * KN / 60.0f);
seas726 49:7da71f479dac 38 trajectoryPlanners[0]->setProfileAcceleration(10.0f);
seas726 49:7da71f479dac 39 trajectoryPlanners[1]->setProfileAcceleration(10.0f);
seas726 49:7da71f479dac 40 trajectoryPlanners[0]->setProfileDeceleration(10.0f);
seas726 49:7da71f479dac 41 trajectoryPlanners[1]->setProfileDeceleration(10.0f);
seas726 49:7da71f479dac 42
seas726 49:7da71f479dac 43 // SPEED CONTROLLERS
seas726 49:7da71f479dac 44 speedControllers[0] = new SpeedController(COUNTS_PER_TURN, KN, MAX_MOTOR_VOLTAGE, pwm_M1, encoder_M1);
seas726 49:7da71f479dac 45 speedControllers[1] = new SpeedController(COUNTS_PER_TURN, KN, MAX_MOTOR_VOLTAGE, pwm_M2, encoder_M2);
seas726 49:7da71f479dac 46
seas726 49:7da71f479dac 47 speedControllers[0]->setSpeedCntrlGain(0.04f); // adjust speedcontroller gains
seas726 49:7da71f479dac 48 speedControllers[1]->setSpeedCntrlGain(0.04f);
seas726 49:7da71f479dac 49 speedControllers[0]->setMaxAccelerationRPS(999.0f); // adjust max. acceleration for smooth movement
seas726 49:7da71f479dac 50 speedControllers[1]->setMaxAccelerationRPS(999.0f);
seas726 49:7da71f479dac 51
seas726 49:7da71f479dac 52 }
seas726 49:7da71f479dac 53
eversonrosed 54:b442660523df 54 Robot::~Robot()
eversonrosed 54:b442660523df 55 {
eversonrosed 54:b442660523df 56 delete trajectoryPlanners[0];
eversonrosed 54:b442660523df 57 delete trajectoryPlanners[1];
eversonrosed 54:b442660523df 58 delete speedControllers[0];
eversonrosed 54:b442660523df 59 delete speedControllers[1];
eversonrosed 54:b442660523df 60 }
eversonrosed 54:b442660523df 61
seas726 49:7da71f479dac 62 void Robot::Update() {
seas726 49:7da71f479dac 63
seas726 49:7da71f479dac 64 controller.Update();
seas726 49:7da71f479dac 65
seas726 49:7da71f479dac 66 printf("STATE: %d \r\n", state);
seas726 49:7da71f479dac 67 switch (state) {
seas726 49:7da71f479dac 68 case INITIAL:
seas726 49:7da71f479dac 69 Initial();
seas726 49:7da71f479dac 70 break;
seas726 49:7da71f479dac 71 case IDLE:
seas726 49:7da71f479dac 72 Idle();
seas726 49:7da71f479dac 73 break;
seas726 49:7da71f479dac 74 case FOLLOWING_LINE:
seas726 49:7da71f479dac 75 FollowingLine();
seas726 49:7da71f479dac 76 break;
seas726 49:7da71f479dac 77 case RIGHT_TURN_90:
seas726 49:7da71f479dac 78 RightTurn();
seas726 49:7da71f479dac 79 break;
seas726 49:7da71f479dac 80 case LEFT_TURN_90:
seas726 49:7da71f479dac 81 LeftTurn();
seas726 49:7da71f479dac 82 break;
seas726 49:7da71f479dac 83 default: state = IDLE; // on default, stop the car
seas726 49:7da71f479dac 84 }
seas726 49:7da71f479dac 85 }
seas726 49:7da71f479dac 86
seas726 49:7da71f479dac 87 void Robot::Initial()
seas726 49:7da71f479dac 88 {
seas726 49:7da71f479dac 89 printf("Initial State\n"); // TODO: REMOVE PRINT
seas726 49:7da71f479dac 90 // initialize the robot.
seas726 49:7da71f479dac 91 // enable_motors = 1;
seas726 49:7da71f479dac 92 // motors_enabled = false;
seas726 49:7da71f479dac 93 robot_speed_desired(0) = 0.0f; // set speed and rotational velocity to zero
seas726 49:7da71f479dac 94 robot_speed_desired(1) = 0.0f;
seas726 49:7da71f479dac 95
seas726 49:7da71f479dac 96 if(controller.GetTurnedOn()) // check to see if blue button is toggled
seas726 49:7da71f479dac 97 {
seas726 49:7da71f479dac 98 state = FOLLOWING_LINE;
seas726 49:7da71f479dac 99 }
seas726 49:7da71f479dac 100 }
seas726 49:7da71f479dac 101
seas726 49:7da71f479dac 102 void Robot::Idle()
seas726 49:7da71f479dac 103 {
seas726 49:7da71f479dac 104 printf("Idle\n"); // TODO: REMOVE PRINT
seas726 49:7da71f479dac 105 robot_speed_desired(0) = 0.0f; // set speed and rotational velocity to zero
seas726 49:7da71f479dac 106 robot_speed_desired(1) = 0.0f;
seas726 49:7da71f479dac 107 }
seas726 49:7da71f479dac 108
seas726 49:7da71f479dac 109 void Robot::FollowingLine()
seas726 49:7da71f479dac 110 {
seas726 49:7da71f479dac 111 printf("FollowingLine\n"); // TODO: REMOVE PRINT
seas726 49:7da71f479dac 112 robot_speed_desired(0) = TRANSLATIONAL_VELOCITY;
seas726 49:7da71f479dac 113 robot_speed_desired(1) = 0.0f; // set
seas726 49:7da71f479dac 114
seas726 49:7da71f479dac 115 printf("%d", line_sensor.getRaw());
seas726 49:7da71f479dac 116
seas726 49:7da71f479dac 117
seas726 49:7da71f479dac 118 // somehow make veloity relative to the angle of the line?
seas726 49:7da71f479dac 119
seas726 49:7da71f479dac 120 }
seas726 49:7da71f479dac 121
seas726 49:7da71f479dac 122
seas726 49:7da71f479dac 123