Psi Swarm Robot / PRRobot

Practical Robotics Modular Robot Library

Dependents:   ModularRobot

motors.cpp@6:732aa91eb555, 2017-01-13 (annotated)

Committer:
jah128
Date:
Fri Jan 13 23:16:23 2017 +0000
Revision:
6:732aa91eb555
Parent:
5:6da8daaeb9f7 
Updated;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jah128 0:8a2dd255c508 1 #include "robot.h"
jah128 0:8a2dd255c508 2
jah128 0:8a2dd255c508 3 DigitalOut nsleep(p7);
jah128 0:8a2dd255c508 4 DigitalIn nfault_l(p8,PullUp);
jah128 0:8a2dd255c508 5 DigitalIn nfault_r(p11,PullUp);
jah128 0:8a2dd255c508 6 AnalogIn motor_vprop_l(p19);
jah128 0:8a2dd255c508 7 AnalogIn motor_vprop_r(p20);
jah128 0:8a2dd255c508 8 PwmOut enable_l(p21);
jah128 0:8a2dd255c508 9 PwmOut enable_r(p22);
jah128 0:8a2dd255c508 10 DigitalOut hb1_l(p23);
jah128 0:8a2dd255c508 11 DigitalOut hb1_r(p24);
jah128 0:8a2dd255c508 12 DigitalOut hb2_l(p29);
jah128 0:8a2dd255c508 13 DigitalOut hb2_r(p30);
jah128 3:8762f6b2ea8d 14 float left_motor_speed = 0;
jah128 3:8762f6b2ea8d 15 float right_motor_speed = 0;
jah128 5:6da8daaeb9f7 16 int motor_pwm_period = 0;
jah128 3:8762f6b2ea8d 17
jah128 3:8762f6b2ea8d 18 float Motors::get_left_motor_speed()
jah128 3:8762f6b2ea8d 19 {
jah128 6:732aa91eb555 20 return left_motor_speed;
jah128 3:8762f6b2ea8d 21 }
jah128 3:8762f6b2ea8d 22
jah128 3:8762f6b2ea8d 23 float Motors::get_right_motor_speed()
jah128 3:8762f6b2ea8d 24 {
jah128 6:732aa91eb555 25 return right_motor_speed;
jah128 3:8762f6b2ea8d 26 }
jah128 3:8762f6b2ea8d 27
jah128 6:732aa91eb555 28 void Motors::forwards(float speed)
jah128 6:732aa91eb555 29 {
jah128 3:8762f6b2ea8d 30 set_left_motor_speed(speed);
jah128 6:732aa91eb555 31 set_right_motor_speed(speed);
jah128 3:8762f6b2ea8d 32 }
jah128 3:8762f6b2ea8d 33
jah128 6:732aa91eb555 34 void Motors::backwards(float speed)
jah128 6:732aa91eb555 35 {
jah128 3:8762f6b2ea8d 36 set_left_motor_speed(-speed);
jah128 6:732aa91eb555 37 set_right_motor_speed(-speed);
jah128 3:8762f6b2ea8d 38 }
jah128 3:8762f6b2ea8d 39
jah128 6:732aa91eb555 40 void Motors::turn(float speed)
jah128 6:732aa91eb555 41 {
jah128 3:8762f6b2ea8d 42 set_left_motor_speed(speed);
jah128 6:732aa91eb555 43 set_right_motor_speed(-speed);
jah128 3:8762f6b2ea8d 44 }
jah128 0:8a2dd255c508 45
jah128 6:732aa91eb555 46 float Motors::get_current_left()
jah128 6:732aa91eb555 47 {
jah128 0:8a2dd255c508 48 float current = motor_vprop_l.read();
jah128 0:8a2dd255c508 49 //Vprop = 5x voltage on sense pins - sense pins have 0.25 ohm resistance - current is 0.8 actual voltage measured [or 2.64 x scaled value]
jah128 6:732aa91eb555 50 return current * 2.64f;
jah128 0:8a2dd255c508 51 }
jah128 0:8a2dd255c508 52
jah128 6:732aa91eb555 53 float Motors::get_current_right()
jah128 6:732aa91eb555 54 {
jah128 0:8a2dd255c508 55 float current = motor_vprop_r.read();
jah128 6:732aa91eb555 56 return current * 2.64f;
jah128 0:8a2dd255c508 57 }
jah128 0:8a2dd255c508 58
jah128 6:732aa91eb555 59 float Motors::get_adjusted_speed(float speed_in)
jah128 6:732aa91eb555 60 {
jah128 0:8a2dd255c508 61 float ret_speed = speed_in;
jah128 6:732aa91eb555 62 if(USE_STALL_OFFSET == 1) {
jah128 0:8a2dd255c508 63 ret_speed += STALL_OFFSET;
jah128 6:732aa91eb555 64 ret_speed /= (STALL_OFFSET + 1.0f);
jah128 6:732aa91eb555 65 }
jah128 0:8a2dd255c508 66 return ret_speed;
jah128 0:8a2dd255c508 67 }
jah128 0:8a2dd255c508 68
jah128 6:732aa91eb555 69 void Motors::sleep()
jah128 6:732aa91eb555 70 {
jah128 6:732aa91eb555 71 nsleep = 0;
jah128 3:8762f6b2ea8d 72 left_motor_speed = 0;
jah128 3:8762f6b2ea8d 73 right_motor_speed = 0;
jah128 0:8a2dd255c508 74 }
jah128 0:8a2dd255c508 75
jah128 6:732aa91eb555 76 void Motors::wake_up()
jah128 6:732aa91eb555 77 {
jah128 6:732aa91eb555 78 nsleep = 1;
jah128 0:8a2dd255c508 79 }
jah128 0:8a2dd255c508 80
jah128 6:732aa91eb555 81 void Motors::coast_left()
jah128 6:732aa91eb555 82 {
jah128 6:732aa91eb555 83 enable_l = 0;
jah128 3:8762f6b2ea8d 84 left_motor_speed = 0;
jah128 0:8a2dd255c508 85 }
jah128 0:8a2dd255c508 86
jah128 6:732aa91eb555 87 void Motors::brake_left()
jah128 6:732aa91eb555 88 {
jah128 0:8a2dd255c508 89 hb1_l = 1;
jah128 0:8a2dd255c508 90 hb2_l = 1;
jah128 6:732aa91eb555 91 enable_l = 1;
jah128 3:8762f6b2ea8d 92 left_motor_speed = 0;
jah128 0:8a2dd255c508 93 }
jah128 0:8a2dd255c508 94
jah128 6:732aa91eb555 95 void Motors::set_left_motor_speed(float speed)
jah128 6:732aa91eb555 96 {
jah128 6:732aa91eb555 97 if(speed == 0) {
jah128 6:732aa91eb555 98 left_motor_speed = 0;
jah128 6:732aa91eb555 99 enable_l = 0;
jah128 6:732aa91eb555 100 } else {
jah128 6:732aa91eb555 101 if(speed < 0) {
jah128 6:732aa91eb555 102 hb1_l = 1;
jah128 6:732aa91eb555 103 hb2_l = 0;
jah128 6:732aa91eb555 104 left_motor_speed = -get_adjusted_speed(-speed);
jah128 6:732aa91eb555 105 enable_l = -left_motor_speed;
jah128 6:732aa91eb555 106 } else {
jah128 6:732aa91eb555 107 hb1_l = 0;
jah128 6:732aa91eb555 108 hb2_l = 1;
jah128 6:732aa91eb555 109 left_motor_speed = get_adjusted_speed(speed);
jah128 6:732aa91eb555 110 enable_l = left_motor_speed;
jah128 6:732aa91eb555 111 }
jah128 6:732aa91eb555 112 }
jah128 0:8a2dd255c508 113 }
jah128 0:8a2dd255c508 114
jah128 6:732aa91eb555 115 void Motors::coast_right()
jah128 6:732aa91eb555 116 {
jah128 6:732aa91eb555 117 enable_r = 0;
jah128 3:8762f6b2ea8d 118 right_motor_speed = 0;
jah128 0:8a2dd255c508 119 }
jah128 0:8a2dd255c508 120
jah128 6:732aa91eb555 121 void Motors::brake_right()
jah128 6:732aa91eb555 122 {
jah128 0:8a2dd255c508 123 hb1_r = 1;
jah128 0:8a2dd255c508 124 hb2_r = 1;
jah128 6:732aa91eb555 125 enable_r = 1;
jah128 3:8762f6b2ea8d 126 right_motor_speed = 0;
jah128 0:8a2dd255c508 127 }
jah128 0:8a2dd255c508 128
jah128 6:732aa91eb555 129 void Motors::coast()
jah128 6:732aa91eb555 130 {
jah128 0:8a2dd255c508 131 coast_left();
jah128 6:732aa91eb555 132 coast_right();
jah128 0:8a2dd255c508 133 }
jah128 0:8a2dd255c508 134
jah128 6:732aa91eb555 135 void Motors::brake()
jah128 6:732aa91eb555 136 {
jah128 0:8a2dd255c508 137 brake_left();
jah128 6:732aa91eb555 138 brake_right();
jah128 0:8a2dd255c508 139 }
jah128 0:8a2dd255c508 140
jah128 6:732aa91eb555 141 void Motors::set_right_motor_speed(float speed)
jah128 6:732aa91eb555 142 {
jah128 6:732aa91eb555 143 if(speed == 0) {
jah128 6:732aa91eb555 144 right_motor_speed = 0;
jah128 6:732aa91eb555 145 enable_r = 0;
jah128 6:732aa91eb555 146 } else {
jah128 6:732aa91eb555 147 if(speed < 0) {
jah128 6:732aa91eb555 148 hb1_r = 1;
jah128 6:732aa91eb555 149 hb2_r = 0;
jah128 6:732aa91eb555 150 right_motor_speed = -get_adjusted_speed(-speed);
jah128 6:732aa91eb555 151 enable_r = -right_motor_speed;
jah128 6:732aa91eb555 152 } else {
jah128 6:732aa91eb555 153 hb1_r = 0;
jah128 6:732aa91eb555 154 hb2_r = 1;
jah128 6:732aa91eb555 155 right_motor_speed = get_adjusted_speed(speed);
jah128 6:732aa91eb555 156 enable_r = right_motor_speed;
jah128 6:732aa91eb555 157 }
jah128 6:732aa91eb555 158 }
jah128 0:8a2dd255c508 159 }
jah128 0:8a2dd255c508 160
jah128 6:732aa91eb555 161 void Motors::set_pwm_period(int period_in)
jah128 6:732aa91eb555 162 {
jah128 5:6da8daaeb9f7 163 motor_pwm_period = period_in;
jah128 5:6da8daaeb9f7 164 enable_l.period_us(motor_pwm_period);
jah128 6:732aa91eb555 165 enable_r.period_us(motor_pwm_period);
jah128 5:6da8daaeb9f7 166 }
jah128 0:8a2dd255c508 167
jah128 6:732aa91eb555 168 void Motors::init()
jah128 6:732aa91eb555 169 {
jah128 5:6da8daaeb9f7 170 if(motor_pwm_period == 0) motor_pwm_period = MOTOR_PWM_PERIOD_US;
jah128 5:6da8daaeb9f7 171 enable_l.period_us(motor_pwm_period);
jah128 5:6da8daaeb9f7 172 enable_r.period_us(motor_pwm_period);
jah128 0:8a2dd255c508 173 enable_l = 0;
jah128 0:8a2dd255c508 174 enable_r = 0;
jah128 0:8a2dd255c508 175 wake_up();
jah128 0:8a2dd255c508 176 }