Psi Swarm Robot / PRRobot

Practical Robotics Modular Robot Library

Dependents:   ModularRobot

motors.cpp@5:6da8daaeb9f7, 2017-01-02 (annotated)

Committer:
jah128
Date:
Mon Jan 02 22:56:34 2017 +0000
Revision:
5:6da8daaeb9f7
Parent:
3:8762f6b2ea8d
Child:
6:732aa91eb555
Add motor pwm serial command;

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 3:8762f6b2ea8d 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 3:8762f6b2ea8d 25 return right_motor_speed;
jah128 3:8762f6b2ea8d 26 }
jah128 3:8762f6b2ea8d 27
jah128 3:8762f6b2ea8d 28 void Motors::forwards(float speed){
jah128 3:8762f6b2ea8d 29 set_left_motor_speed(speed);
jah128 3:8762f6b2ea8d 30 set_right_motor_speed(speed);
jah128 3:8762f6b2ea8d 31 }
jah128 3:8762f6b2ea8d 32
jah128 3:8762f6b2ea8d 33 void Motors::backwards(float speed){
jah128 3:8762f6b2ea8d 34 set_left_motor_speed(-speed);
jah128 3:8762f6b2ea8d 35 set_right_motor_speed(-speed);
jah128 3:8762f6b2ea8d 36 }
jah128 3:8762f6b2ea8d 37
jah128 3:8762f6b2ea8d 38 void Motors::turn(float speed){
jah128 3:8762f6b2ea8d 39 set_left_motor_speed(speed);
jah128 3:8762f6b2ea8d 40 set_right_motor_speed(-speed);
jah128 3:8762f6b2ea8d 41 }
jah128 0:8a2dd255c508 42
jah128 0:8a2dd255c508 43 float Motors::get_current_left(){
jah128 0:8a2dd255c508 44 float current = motor_vprop_l.read();
jah128 0:8a2dd255c508 45 //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 0:8a2dd255c508 46 return current * 2.64f;
jah128 0:8a2dd255c508 47 }
jah128 0:8a2dd255c508 48
jah128 0:8a2dd255c508 49 float Motors::get_current_right(){
jah128 0:8a2dd255c508 50 float current = motor_vprop_r.read();
jah128 0:8a2dd255c508 51 return current * 2.64f;
jah128 0:8a2dd255c508 52 }
jah128 0:8a2dd255c508 53
jah128 0:8a2dd255c508 54 float Motors::get_adjusted_speed(float speed_in){
jah128 0:8a2dd255c508 55 float ret_speed = speed_in;
jah128 0:8a2dd255c508 56 if(USE_STALL_OFFSET == 1){
jah128 0:8a2dd255c508 57 ret_speed += STALL_OFFSET;
jah128 0:8a2dd255c508 58 ret_speed /= (STALL_OFFSET + 1.0f);
jah128 0:8a2dd255c508 59 }
jah128 0:8a2dd255c508 60 return ret_speed;
jah128 0:8a2dd255c508 61 }
jah128 0:8a2dd255c508 62
jah128 0:8a2dd255c508 63 void Motors::sleep(){
jah128 0:8a2dd255c508 64 nsleep = 0;
jah128 3:8762f6b2ea8d 65 left_motor_speed = 0;
jah128 3:8762f6b2ea8d 66 right_motor_speed = 0;
jah128 0:8a2dd255c508 67 }
jah128 0:8a2dd255c508 68
jah128 0:8a2dd255c508 69 void Motors::wake_up(){
jah128 0:8a2dd255c508 70 nsleep = 1;
jah128 0:8a2dd255c508 71 }
jah128 0:8a2dd255c508 72
jah128 0:8a2dd255c508 73 void Motors::coast_left(){
jah128 0:8a2dd255c508 74 enable_l = 0;
jah128 3:8762f6b2ea8d 75 left_motor_speed = 0;
jah128 0:8a2dd255c508 76 }
jah128 0:8a2dd255c508 77
jah128 0:8a2dd255c508 78 void Motors::brake_left(){
jah128 0:8a2dd255c508 79 hb1_l = 1;
jah128 0:8a2dd255c508 80 hb2_l = 1;
jah128 0:8a2dd255c508 81 enable_l = 1;
jah128 3:8762f6b2ea8d 82 left_motor_speed = 0;
jah128 0:8a2dd255c508 83 }
jah128 0:8a2dd255c508 84
jah128 0:8a2dd255c508 85 void Motors::set_left_motor_speed(float speed){
jah128 0:8a2dd255c508 86 if(speed < 0){
jah128 0:8a2dd255c508 87 hb1_l = 0;
jah128 0:8a2dd255c508 88 hb2_l = 1;
jah128 3:8762f6b2ea8d 89 left_motor_speed = -get_adjusted_speed(-speed);
jah128 3:8762f6b2ea8d 90 enable_l = -left_motor_speed;
jah128 0:8a2dd255c508 91 }else{
jah128 0:8a2dd255c508 92 hb1_l = 1;
jah128 0:8a2dd255c508 93 hb2_l = 0;
jah128 3:8762f6b2ea8d 94 left_motor_speed = get_adjusted_speed(speed);
jah128 3:8762f6b2ea8d 95 enable_l = left_motor_speed;
jah128 0:8a2dd255c508 96 }
jah128 0:8a2dd255c508 97 }
jah128 0:8a2dd255c508 98
jah128 0:8a2dd255c508 99 void Motors::coast_right(){
jah128 0:8a2dd255c508 100 enable_r = 0;
jah128 3:8762f6b2ea8d 101 right_motor_speed = 0;
jah128 0:8a2dd255c508 102 }
jah128 0:8a2dd255c508 103
jah128 0:8a2dd255c508 104 void Motors::brake_right(){
jah128 0:8a2dd255c508 105 hb1_r = 1;
jah128 0:8a2dd255c508 106 hb2_r = 1;
jah128 0:8a2dd255c508 107 enable_r = 1;
jah128 3:8762f6b2ea8d 108 right_motor_speed = 0;
jah128 0:8a2dd255c508 109 }
jah128 0:8a2dd255c508 110
jah128 0:8a2dd255c508 111 void Motors::coast(){
jah128 0:8a2dd255c508 112 coast_left();
jah128 0:8a2dd255c508 113 coast_right();
jah128 0:8a2dd255c508 114 }
jah128 0:8a2dd255c508 115
jah128 0:8a2dd255c508 116 void Motors::brake(){
jah128 0:8a2dd255c508 117 brake_left();
jah128 0:8a2dd255c508 118 brake_right();
jah128 0:8a2dd255c508 119 }
jah128 0:8a2dd255c508 120
jah128 0:8a2dd255c508 121 void Motors::set_right_motor_speed(float speed){
jah128 0:8a2dd255c508 122 if(speed < 0){
jah128 0:8a2dd255c508 123 hb1_r = 0;
jah128 0:8a2dd255c508 124 hb2_r = 1;
jah128 3:8762f6b2ea8d 125 right_motor_speed = -get_adjusted_speed(-speed);
jah128 3:8762f6b2ea8d 126 enable_r = -right_motor_speed;
jah128 0:8a2dd255c508 127 }else{
jah128 0:8a2dd255c508 128 hb1_r = 1;
jah128 0:8a2dd255c508 129 hb2_r = 0;
jah128 3:8762f6b2ea8d 130 right_motor_speed = get_adjusted_speed(speed);
jah128 3:8762f6b2ea8d 131 enable_r = right_motor_speed;
jah128 0:8a2dd255c508 132 }
jah128 0:8a2dd255c508 133 }
jah128 0:8a2dd255c508 134
jah128 5:6da8daaeb9f7 135 void Motors::set_pwm_period(int period_in){
jah128 5:6da8daaeb9f7 136 motor_pwm_period = period_in;
jah128 5:6da8daaeb9f7 137 enable_l.period_us(motor_pwm_period);
jah128 5:6da8daaeb9f7 138 enable_r.period_us(motor_pwm_period);
jah128 5:6da8daaeb9f7 139 }
jah128 0:8a2dd255c508 140
jah128 0:8a2dd255c508 141 void Motors::init(){
jah128 5:6da8daaeb9f7 142 if(motor_pwm_period == 0) motor_pwm_period = MOTOR_PWM_PERIOD_US;
jah128 5:6da8daaeb9f7 143 enable_l.period_us(motor_pwm_period);
jah128 5:6da8daaeb9f7 144 enable_r.period_us(motor_pwm_period);
jah128 0:8a2dd255c508 145 enable_l = 0;
jah128 0:8a2dd255c508 146 enable_r = 0;
jah128 0:8a2dd255c508 147 wake_up();
jah128 0:8a2dd255c508 148 }