にいむら にいむら / Mbed 2 deprecated mainboardnrp2018

unkounko

Dependencies:   mbed Servo

Output/Motor/Motor.cpp@0:562021ed1ba9, 2018-01-13 (annotated)

Committer:
t_yamamoto
Date:
Sat Jan 13 13:33:09 2018 +0000
Revision:
0:562021ed1ba9
Child:
1:e73cf2469f83
NaganoNCT-MainBoardNRP2018

Who changed what in which revision?

UserRevisionLine numberNew contents of line
t_yamamoto 0:562021ed1ba9 1 #include "Motor.h"
t_yamamoto 0:562021ed1ba9 2
t_yamamoto 0:562021ed1ba9 3 #include <stdint.h>
t_yamamoto 0:562021ed1ba9 4 #include "mbed.h"
t_yamamoto 0:562021ed1ba9 5
t_yamamoto 0:562021ed1ba9 6 namespace MOTOR
t_yamamoto 0:562021ed1ba9 7 {
t_yamamoto 0:562021ed1ba9 8 namespace
t_yamamoto 0:562021ed1ba9 9 {
t_yamamoto 0:562021ed1ba9 10 MotorStatus motor[MOUNTING_MOTOR_NUM];
t_yamamoto 0:562021ed1ba9 11 DigitalOut directions[] = {
t_yamamoto 0:562021ed1ba9 12 DigitalOut(MOTOR0_D1_PIN),
t_yamamoto 0:562021ed1ba9 13 DigitalOut(MOTOR0_D2_PIN),
t_yamamoto 0:562021ed1ba9 14 DigitalOut(MOTOR1_D1_PIN),
t_yamamoto 0:562021ed1ba9 15 DigitalOut(MOTOR1_D2_PIN),
t_yamamoto 0:562021ed1ba9 16 DigitalOut(MOTOR2_D1_PIN),
t_yamamoto 0:562021ed1ba9 17 DigitalOut(MOTOR2_D2_PIN),
t_yamamoto 0:562021ed1ba9 18 DigitalOut(MOTOR3_D1_PIN),
t_yamamoto 0:562021ed1ba9 19 DigitalOut(MOTOR3_D2_PIN),
t_yamamoto 0:562021ed1ba9 20 DigitalOut(MOTOR4_D1_PIN),
t_yamamoto 0:562021ed1ba9 21 DigitalOut(MOTOR4_D2_PIN),
t_yamamoto 0:562021ed1ba9 22 DigitalOut(MOTOR5_D1_PIN),
t_yamamoto 0:562021ed1ba9 23 DigitalOut(MOTOR5_D2_PIN),
t_yamamoto 0:562021ed1ba9 24 };
t_yamamoto 0:562021ed1ba9 25 PwmOut pwms[] = {
t_yamamoto 0:562021ed1ba9 26 PwmOut(MOTOR0_PWM_PIN),
t_yamamoto 0:562021ed1ba9 27 PwmOut(MOTOR1_PWM_PIN),
t_yamamoto 0:562021ed1ba9 28 PwmOut(MOTOR2_PWM_PIN),
t_yamamoto 0:562021ed1ba9 29 PwmOut(MOTOR3_PWM_PIN),
t_yamamoto 0:562021ed1ba9 30 PwmOut(MOTOR4_PWM_PIN),
t_yamamoto 0:562021ed1ba9 31 PwmOut(MOTOR5_PWM_PIN),
t_yamamoto 0:562021ed1ba9 32 };
t_yamamoto 0:562021ed1ba9 33 }
t_yamamoto 0:562021ed1ba9 34
t_yamamoto 0:562021ed1ba9 35 float percentage_to_ratio(float percentage);
t_yamamoto 0:562021ed1ba9 36
t_yamamoto 0:562021ed1ba9 37 void Motor::Initialize(void) {
t_yamamoto 0:562021ed1ba9 38 //Port Initialize
t_yamamoto 0:562021ed1ba9 39 for(uint8_t i = 0; i < MOUNTING_MOTOR_NUM * 2; i++) {
t_yamamoto 0:562021ed1ba9 40 directions[i] = 0;
t_yamamoto 0:562021ed1ba9 41 }
t_yamamoto 0:562021ed1ba9 42
t_yamamoto 0:562021ed1ba9 43 //Pwm Initialize
t_yamamoto 0:562021ed1ba9 44 for(uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) {
t_yamamoto 0:562021ed1ba9 45 pwms[i].period_us(50); //20kHz
t_yamamoto 0:562021ed1ba9 46 pwms[i] = 0.0;
t_yamamoto 0:562021ed1ba9 47 }
t_yamamoto 0:562021ed1ba9 48
t_yamamoto 0:562021ed1ba9 49 SetDefault();
t_yamamoto 0:562021ed1ba9 50 }
t_yamamoto 0:562021ed1ba9 51
t_yamamoto 0:562021ed1ba9 52 void Motor::SetDefault(void) {
t_yamamoto 0:562021ed1ba9 53 for(uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) {
t_yamamoto 0:562021ed1ba9 54 motor[i].dir = FREE;
t_yamamoto 0:562021ed1ba9 55 motor[i].pwm = 0;
t_yamamoto 0:562021ed1ba9 56 }
t_yamamoto 0:562021ed1ba9 57 }
t_yamamoto 0:562021ed1ba9 58
t_yamamoto 0:562021ed1ba9 59 void Motor::Update(MotorStatus *status) {
t_yamamoto 0:562021ed1ba9 60 for(uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) motor[i] = status[i];
t_yamamoto 0:562021ed1ba9 61
t_yamamoto 0:562021ed1ba9 62 //PWM Update
t_yamamoto 0:562021ed1ba9 63 for(uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) pwms[i] = percentage_to_ratio(motor[i].pwm);
t_yamamoto 0:562021ed1ba9 64
t_yamamoto 0:562021ed1ba9 65 //Port Update
t_yamamoto 0:562021ed1ba9 66 for(uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) {
t_yamamoto 0:562021ed1ba9 67 directions[i * 2] = motor[i].d1;
t_yamamoto 0:562021ed1ba9 68 directions[i * 2 + 1] = motor[i].d2;
t_yamamoto 0:562021ed1ba9 69 }
t_yamamoto 0:562021ed1ba9 70 }
t_yamamoto 0:562021ed1ba9 71
t_yamamoto 0:562021ed1ba9 72 float percentage_to_ratio(float percentage) {
t_yamamoto 0:562021ed1ba9 73 return percentage / 100.0;
t_yamamoto 0:562021ed1ba9 74 }
t_yamamoto 0:562021ed1ba9 75
t_yamamoto 0:562021ed1ba9 76 int Motor::SetStatus(float pwm) {
t_yamamoto 0:562021ed1ba9 77 if(pwm > 0.0) return FOR;
t_yamamoto 0:562021ed1ba9 78 else if(pwm < 0.0) return BACK;
t_yamamoto 0:562021ed1ba9 79 else return BRAKE;
t_yamamoto 0:562021ed1ba9 80 }
t_yamamoto 0:562021ed1ba9 81 }
t_yamamoto 0:562021ed1ba9 82