toshi mura / Mbed 2 deprecated NRP2020_main

gjyrjyykkudt

Dependencies:   mbed MCP23017

Motor/Motor.cpp@2:32d2cd7d744b, 2020-01-23 (annotated)

Committer:
M_souta
Date:
Thu Jan 23 09:16:38 2020 +0000
Revision:
2:32d2cd7d744b
Parent:
1:5b0303768126
Child:
4:15547d07f8e5
lll

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) {
M_souta 2:32d2cd7d744b 70 return percentage / 100.0;
M_souta 2:32d2cd7d744b 71 }
M_souta 2:32d2cd7d744b 72
M_souta 2:32d2cd7d744b 73 int Motor::SetStatus(float pwm) {
M_souta 2:32d2cd7d744b 74 if(pwm > 0.0) return FOR;
M_souta 2:32d2cd7d744b 75 else if(pwm < 0.0) 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) {
M_souta 2:32d2cd7d744b 80 if(pwm > 0.0) return pwm;
M_souta 2:32d2cd7d744b 81 else if(pwm < 0.0) 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 }