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

aa

Dependencies:   mbed

Output/Motor/Motor.cpp@0:2e7a61458dc3, 2019-01-28 (annotated)

Committer:
M_souta
Date:
Mon Jan 28 08:34:36 2019 +0000
Revision:
0:2e7a61458dc3
NRP2019

Who changed what in which revision?

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