Psi Swarm Robot / PRRobot

Practical Robotics Modular Robot Library

Dependents:   ModularRobot

motors.cpp@0:8a2dd255c508, 2016-11-26 (annotated)

Committer:
jah128
Date:
Sat Nov 26 17:28:53 2016 +0000
Revision:
0:8a2dd255c508
Child:
3:8762f6b2ea8d
Initial commit

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 0:8a2dd255c508 14
jah128 0:8a2dd255c508 15 float Motors::get_current_left(){
jah128 0:8a2dd255c508 16 float current = motor_vprop_l.read();
jah128 0:8a2dd255c508 17 //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 18 return current * 2.64f;
jah128 0:8a2dd255c508 19 }
jah128 0:8a2dd255c508 20
jah128 0:8a2dd255c508 21 float Motors::get_current_right(){
jah128 0:8a2dd255c508 22 float current = motor_vprop_r.read();
jah128 0:8a2dd255c508 23 return current * 2.64f;
jah128 0:8a2dd255c508 24 }
jah128 0:8a2dd255c508 25
jah128 0:8a2dd255c508 26 float Motors::get_adjusted_speed(float speed_in){
jah128 0:8a2dd255c508 27 float ret_speed = speed_in;
jah128 0:8a2dd255c508 28 if(USE_STALL_OFFSET == 1){
jah128 0:8a2dd255c508 29 ret_speed += STALL_OFFSET;
jah128 0:8a2dd255c508 30 ret_speed /= (STALL_OFFSET + 1.0f);
jah128 0:8a2dd255c508 31 }
jah128 0:8a2dd255c508 32 return ret_speed;
jah128 0:8a2dd255c508 33 }
jah128 0:8a2dd255c508 34
jah128 0:8a2dd255c508 35 void Motors::sleep(){
jah128 0:8a2dd255c508 36 nsleep = 0;
jah128 0:8a2dd255c508 37 }
jah128 0:8a2dd255c508 38
jah128 0:8a2dd255c508 39 void Motors::wake_up(){
jah128 0:8a2dd255c508 40 nsleep = 1;
jah128 0:8a2dd255c508 41 }
jah128 0:8a2dd255c508 42
jah128 0:8a2dd255c508 43 void Motors::coast_left(){
jah128 0:8a2dd255c508 44 enable_l = 0;
jah128 0:8a2dd255c508 45 }
jah128 0:8a2dd255c508 46
jah128 0:8a2dd255c508 47 void Motors::brake_left(){
jah128 0:8a2dd255c508 48 hb1_l = 1;
jah128 0:8a2dd255c508 49 hb2_l = 1;
jah128 0:8a2dd255c508 50 enable_l = 1;
jah128 0:8a2dd255c508 51 }
jah128 0:8a2dd255c508 52
jah128 0:8a2dd255c508 53 void Motors::set_left_motor_speed(float speed){
jah128 0:8a2dd255c508 54 if(speed < 0){
jah128 0:8a2dd255c508 55 hb1_l = 0;
jah128 0:8a2dd255c508 56 hb2_l = 1;
jah128 0:8a2dd255c508 57 enable_l = get_adjusted_speed(-speed);
jah128 0:8a2dd255c508 58 }else{
jah128 0:8a2dd255c508 59 hb1_l = 1;
jah128 0:8a2dd255c508 60 hb2_l = 0;
jah128 0:8a2dd255c508 61 enable_l = get_adjusted_speed(speed);
jah128 0:8a2dd255c508 62 }
jah128 0:8a2dd255c508 63 }
jah128 0:8a2dd255c508 64
jah128 0:8a2dd255c508 65 void Motors::coast_right(){
jah128 0:8a2dd255c508 66 enable_r = 0;
jah128 0:8a2dd255c508 67 }
jah128 0:8a2dd255c508 68
jah128 0:8a2dd255c508 69 void Motors::brake_right(){
jah128 0:8a2dd255c508 70 hb1_r = 1;
jah128 0:8a2dd255c508 71 hb2_r = 1;
jah128 0:8a2dd255c508 72 enable_r = 1;
jah128 0:8a2dd255c508 73 }
jah128 0:8a2dd255c508 74
jah128 0:8a2dd255c508 75 void Motors::coast(){
jah128 0:8a2dd255c508 76 coast_left();
jah128 0:8a2dd255c508 77 coast_right();
jah128 0:8a2dd255c508 78 }
jah128 0:8a2dd255c508 79
jah128 0:8a2dd255c508 80 void Motors::brake(){
jah128 0:8a2dd255c508 81 brake_left();
jah128 0:8a2dd255c508 82 brake_right();
jah128 0:8a2dd255c508 83 }
jah128 0:8a2dd255c508 84
jah128 0:8a2dd255c508 85 void Motors::set_right_motor_speed(float speed){
jah128 0:8a2dd255c508 86 if(speed < 0){
jah128 0:8a2dd255c508 87 hb1_r = 0;
jah128 0:8a2dd255c508 88 hb2_r = 1;
jah128 0:8a2dd255c508 89 enable_r = get_adjusted_speed(-speed);
jah128 0:8a2dd255c508 90 }else{
jah128 0:8a2dd255c508 91 hb1_r = 1;
jah128 0:8a2dd255c508 92 hb2_r = 0;
jah128 0:8a2dd255c508 93 enable_r = get_adjusted_speed(speed);
jah128 0:8a2dd255c508 94 }
jah128 0:8a2dd255c508 95 }
jah128 0:8a2dd255c508 96
jah128 0:8a2dd255c508 97
jah128 0:8a2dd255c508 98 void Motors::init(){
jah128 0:8a2dd255c508 99 enable_l.period_us(MOTOR_PWM_PERIOD_US);
jah128 0:8a2dd255c508 100 enable_r.period_us(MOTOR_PWM_PERIOD_US);
jah128 0:8a2dd255c508 101 enable_l = 0;
jah128 0:8a2dd255c508 102 enable_r = 0;
jah128 0:8a2dd255c508 103 wake_up();
jah128 0:8a2dd255c508 104 }