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

unkounko

Dependencies:   mbed Servo

Output/Motor/Motor.cpp@8:cb53beff4bb2, 2018-02-24 (annotated)

Committer:
t_yamamoto
Date:
Sat Feb 24 07:32:11 2018 +0000
Revision:
8:cb53beff4bb2
Parent:
1:e73cf2469f83 
Sensor

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 };
t_yamamoto 0:562021ed1ba9 23 PwmOut pwms[] = {
t_yamamoto 0:562021ed1ba9 24 PwmOut(MOTOR0_PWM_PIN),
t_yamamoto 0:562021ed1ba9 25 PwmOut(MOTOR1_PWM_PIN),
t_yamamoto 0:562021ed1ba9 26 PwmOut(MOTOR2_PWM_PIN),
t_yamamoto 0:562021ed1ba9 27 PwmOut(MOTOR3_PWM_PIN),
t_yamamoto 0:562021ed1ba9 28 PwmOut(MOTOR4_PWM_PIN),
t_yamamoto 0:562021ed1ba9 29 };
t_yamamoto 0:562021ed1ba9 30 }
t_yamamoto 0:562021ed1ba9 31
t_yamamoto 0:562021ed1ba9 32 float percentage_to_ratio(float percentage);
t_yamamoto 0:562021ed1ba9 33
t_yamamoto 0:562021ed1ba9 34 void Motor::Initialize(void) {
t_yamamoto 0:562021ed1ba9 35 //Port Initialize
t_yamamoto 8:cb53beff4bb2 36 for(uint8_t i = MOTOR_START_NUM; i < MOUNTING_MOTOR_NUM * 2; i++) {
t_yamamoto 0:562021ed1ba9 37 directions[i] = 0;
t_yamamoto 0:562021ed1ba9 38 }
t_yamamoto 0:562021ed1ba9 39
t_yamamoto 0:562021ed1ba9 40 //Pwm Initialize
t_yamamoto 8:cb53beff4bb2 41 for(uint8_t i = MOTOR_START_NUM; i < MOUNTING_MOTOR_NUM; i++) {
t_yamamoto 0:562021ed1ba9 42 pwms[i].period_us(50); //20kHz
t_yamamoto 0:562021ed1ba9 43 pwms[i] = 0.0;
t_yamamoto 0:562021ed1ba9 44 }
t_yamamoto 0:562021ed1ba9 45
t_yamamoto 0:562021ed1ba9 46 SetDefault();
t_yamamoto 0:562021ed1ba9 47 }
t_yamamoto 0:562021ed1ba9 48
t_yamamoto 0:562021ed1ba9 49 void Motor::SetDefault(void) {
t_yamamoto 8:cb53beff4bb2 50 for(uint8_t i = MOTOR_START_NUM; i < MOUNTING_MOTOR_NUM; i++) {
t_yamamoto 0:562021ed1ba9 51 motor[i].dir = FREE;
t_yamamoto 0:562021ed1ba9 52 motor[i].pwm = 0;
t_yamamoto 0:562021ed1ba9 53 }
t_yamamoto 0:562021ed1ba9 54 }
t_yamamoto 0:562021ed1ba9 55
t_yamamoto 0:562021ed1ba9 56 void Motor::Update(MotorStatus *status) {
t_yamamoto 8:cb53beff4bb2 57 for(uint8_t i = MOTOR_START_NUM; i < MOUNTING_MOTOR_NUM; i++) motor[i] = status[i];
t_yamamoto 0:562021ed1ba9 58
t_yamamoto 0:562021ed1ba9 59 //PWM Update
t_yamamoto 8:cb53beff4bb2 60 for(uint8_t i = MOTOR_START_NUM; i < MOUNTING_MOTOR_NUM; i++) pwms[i] = percentage_to_ratio(motor[i].pwm);
t_yamamoto 0:562021ed1ba9 61
t_yamamoto 0:562021ed1ba9 62 //Port Update
t_yamamoto 8:cb53beff4bb2 63 for(uint8_t i = MOTOR_START_NUM; i < MOUNTING_MOTOR_NUM; i++) {
t_yamamoto 0:562021ed1ba9 64 directions[i * 2] = motor[i].d1;
t_yamamoto 0:562021ed1ba9 65 directions[i * 2 + 1] = motor[i].d2;
t_yamamoto 0:562021ed1ba9 66 }
t_yamamoto 0:562021ed1ba9 67 }
t_yamamoto 0:562021ed1ba9 68
t_yamamoto 0:562021ed1ba9 69 float percentage_to_ratio(float percentage) {
t_yamamoto 0:562021ed1ba9 70 return percentage / 100.0;
t_yamamoto 0:562021ed1ba9 71 }
t_yamamoto 0:562021ed1ba9 72
t_yamamoto 0:562021ed1ba9 73 int Motor::SetStatus(float pwm) {
t_yamamoto 0:562021ed1ba9 74 if(pwm > 0.0) return FOR;
t_yamamoto 0:562021ed1ba9 75 else if(pwm < 0.0) return BACK;
t_yamamoto 0:562021ed1ba9 76 else return BRAKE;
t_yamamoto 0:562021ed1ba9 77 }
t_yamamoto 0:562021ed1ba9 78 }