toshi mura / Mbed 2 deprecated NRP2020_main

gjyrjyykkudt

Dependencies:   mbed MCP23017

Motor/Motor.cpp@4:15547d07f8e5, 2020-01-25 (annotated)

Committer:
Ryosei
Date:
Sat Jan 25 00:40:35 2020 +0000
Revision:
4:15547d07f8e5
Parent:
2:32d2cd7d744b 
This is the program, used in NRP.

Who changed what in which revision?

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