Miya Miyagawa / Mbed 2 deprecated Orewotaosita

If you wont to knock me down, look this file...

Dependencies:   mbed Servo

Output/Motor/Motor.cpp@0:4df75b08b14a, 2019-05-03 (annotated)

Committer:
Ryosei
Date:
Fri May 03 11:47:27 2019 +0000
Revision:
0:4df75b08b14a
a

Who changed what in which revision?

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