Psi Swarm Robot / PRRobot

Practical Robotics Modular Robot Library

Dependents:   ModularRobot

motors.cpp@3:8762f6b2ea8d, 2016-11-28 (annotated)

Committer:
jah128
Date:
Mon Nov 28 22:41:14 2016 +0000
Revision:
3:8762f6b2ea8d
Parent:
0:8a2dd255c508
Child:
5:6da8daaeb9f7
Added first RPi input

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