Tetsuya Yamamoto / Mbed 2 deprecated MD-Tutorial

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@0:eed6204ea3b1, 2020-10-30 (annotated)

Committer:
tetsu_0207
Date:
Fri Oct 30 10:44:14 2020 +0000
Revision:
0:eed6204ea3b1
Child:
1:0ef4f75d84b5
Initial commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tetsu_0207 0:eed6204ea3b1 1 #include "mbed.h"
tetsu_0207 0:eed6204ea3b1 2
tetsu_0207 0:eed6204ea3b1 3 // Left Motor
tetsu_0207 0:eed6204ea3b1 4 #define PWM_L A6
tetsu_0207 0:eed6204ea3b1 5 #define DIR_L A3
tetsu_0207 0:eed6204ea3b1 6 // Right Motor
tetsu_0207 0:eed6204ea3b1 7 #define PWM_R A2
tetsu_0207 0:eed6204ea3b1 8 #define DIR_R D12
tetsu_0207 0:eed6204ea3b1 9 // default setting
tetsu_0207 0:eed6204ea3b1 10 #define DIR_DEFAULT_L 1
tetsu_0207 0:eed6204ea3b1 11 #define DIR_DEFAULT_R 0
tetsu_0207 0:eed6204ea3b1 12
tetsu_0207 0:eed6204ea3b1 13 // Math Function
tetsu_0207 0:eed6204ea3b1 14 #define PI 3.14159265359
tetsu_0207 0:eed6204ea3b1 15 // power clock
tetsu_0207 0:eed6204ea3b1 16 #define POWER_CHANGE_PER_CLOCK 5
tetsu_0207 0:eed6204ea3b1 17
tetsu_0207 0:eed6204ea3b1 18 // PWM
tetsu_0207 0:eed6204ea3b1 19 PwmOut pwmL(PWM_L);
tetsu_0207 0:eed6204ea3b1 20 // DIR
tetsu_0207 0:eed6204ea3b1 21 DigitalOut dirL(DIR_L);
tetsu_0207 0:eed6204ea3b1 22 // PWM
tetsu_0207 0:eed6204ea3b1 23 PwmOut pwmR(PWM_R);
tetsu_0207 0:eed6204ea3b1 24 // DIR
tetsu_0207 0:eed6204ea3b1 25 DigitalOut dirR(DIR_R);
tetsu_0207 0:eed6204ea3b1 26 // resistor
tetsu_0207 0:eed6204ea3b1 27 //AnalogIn in(D3);
tetsu_0207 0:eed6204ea3b1 28 // joystick analogin
tetsu_0207 0:eed6204ea3b1 29 AnalogIn joyX(A0);
tetsu_0207 0:eed6204ea3b1 30 AnalogIn joyY(A1);
tetsu_0207 0:eed6204ea3b1 31
tetsu_0207 0:eed6204ea3b1 32 // current setting
tetsu_0207 0:eed6204ea3b1 33 double currentPowerPerL = 0;
tetsu_0207 0:eed6204ea3b1 34 double currentPowerPerR = 0;
tetsu_0207 0:eed6204ea3b1 35 // current power
tetsu_0207 0:eed6204ea3b1 36 double currentPowerL = 0;
tetsu_0207 0:eed6204ea3b1 37 double currentPowerR = 0;
tetsu_0207 0:eed6204ea3b1 38
tetsu_0207 0:eed6204ea3b1 39 int main()
tetsu_0207 0:eed6204ea3b1 40 {
tetsu_0207 0:eed6204ea3b1 41 //printf("start program... \n\r");
tetsu_0207 0:eed6204ea3b1 42 // period
tetsu_0207 0:eed6204ea3b1 43 pwmL.period_us(100);
tetsu_0207 0:eed6204ea3b1 44 pwmR.period_us(100);
tetsu_0207 0:eed6204ea3b1 45 // Dir
tetsu_0207 0:eed6204ea3b1 46 dirL = DIR_DEFAULT_L;
tetsu_0207 0:eed6204ea3b1 47 dirR = DIR_DEFAULT_R;
tetsu_0207 0:eed6204ea3b1 48 // power setting
tetsu_0207 0:eed6204ea3b1 49 float maxPower = 0.9F;
tetsu_0207 0:eed6204ea3b1 50
tetsu_0207 0:eed6204ea3b1 51 //printf("finish start up! \n\r");
tetsu_0207 0:eed6204ea3b1 52
tetsu_0207 0:eed6204ea3b1 53 while(1){
tetsu_0207 0:eed6204ea3b1 54 // prepare power value
tetsu_0207 0:eed6204ea3b1 55 float powerL,powerR;
tetsu_0207 0:eed6204ea3b1 56
tetsu_0207 0:eed6204ea3b1 57 float x = joyX.read(); // go ahead & back
tetsu_0207 0:eed6204ea3b1 58 float y = joyY.read(); // left & right
tetsu_0207 0:eed6204ea3b1 59 // value format
tetsu_0207 0:eed6204ea3b1 60 if(x > 1) x = 1.0;
tetsu_0207 0:eed6204ea3b1 61 if(x < 0) x = 0.0;
tetsu_0207 0:eed6204ea3b1 62 if(y > 1) y = 1.0;
tetsu_0207 0:eed6204ea3b1 63 if(y < 0) y = 0.0;
tetsu_0207 0:eed6204ea3b1 64 // off set
tetsu_0207 0:eed6204ea3b1 65 //if(0.48F < x && x < 0.52F)x = 0.5F;
tetsu_0207 0:eed6204ea3b1 66 //if(0.48F < y && y < 0.52F)y = 0.5F;
tetsu_0207 0:eed6204ea3b1 67
tetsu_0207 0:eed6204ea3b1 68 // format x and y
tetsu_0207 0:eed6204ea3b1 69 double formatX = (2*x) - 1;
tetsu_0207 0:eed6204ea3b1 70 double formatY = (2*y) - 1;
tetsu_0207 0:eed6204ea3b1 71 // tan
tetsu_0207 0:eed6204ea3b1 72 double tan = formatY / formatX;
tetsu_0207 0:eed6204ea3b1 73 // arc tan
tetsu_0207 0:eed6204ea3b1 74 double arctan = (double) atan(tan);
tetsu_0207 0:eed6204ea3b1 75 // angle
tetsu_0207 0:eed6204ea3b1 76 double angle = arctan * (180 / PI);
tetsu_0207 0:eed6204ea3b1 77 // range
tetsu_0207 0:eed6204ea3b1 78 double range = sqrt((formatX * formatX) + (formatY * formatY));
tetsu_0207 0:eed6204ea3b1 79 if(range > 1.0F) range = 1.0F;
tetsu_0207 0:eed6204ea3b1 80 // all power
tetsu_0207 0:eed6204ea3b1 81 double allPower = maxPower * (range / 1);
tetsu_0207 0:eed6204ea3b1 82
tetsu_0207 0:eed6204ea3b1 83 // right left power persent
tetsu_0207 0:eed6204ea3b1 84 double rightPowerPercent,leftPowerPercent;
tetsu_0207 0:eed6204ea3b1 85 int area = 0;
tetsu_0207 0:eed6204ea3b1 86 //1
tetsu_0207 0:eed6204ea3b1 87 if((0 < formatX && formatX < 1) && (0 < formatY && formatY < 1)){
tetsu_0207 0:eed6204ea3b1 88 rightPowerPercent = ((angle / 45) - 1) / 1;
tetsu_0207 0:eed6204ea3b1 89 leftPowerPercent = 1;
tetsu_0207 0:eed6204ea3b1 90
tetsu_0207 0:eed6204ea3b1 91 area = 1;
tetsu_0207 0:eed6204ea3b1 92 }
tetsu_0207 0:eed6204ea3b1 93 //2
tetsu_0207 0:eed6204ea3b1 94 if((formatX < 0 && -1 < formatX) && (0 < formatY && formatY < 1)){
tetsu_0207 0:eed6204ea3b1 95 rightPowerPercent = 1;
tetsu_0207 0:eed6204ea3b1 96 leftPowerPercent = ((-angle / 45) - 1) / 1;
tetsu_0207 0:eed6204ea3b1 97
tetsu_0207 0:eed6204ea3b1 98 area = 2;
tetsu_0207 0:eed6204ea3b1 99 }
tetsu_0207 0:eed6204ea3b1 100 //3
tetsu_0207 0:eed6204ea3b1 101 if((formatX < 0 && -1 < formatX) && (formatY < 0 && -1 < formatY)){
tetsu_0207 0:eed6204ea3b1 102 rightPowerPercent = ((-angle / 45) + 1) / 1;
tetsu_0207 0:eed6204ea3b1 103 leftPowerPercent = -1;
tetsu_0207 0:eed6204ea3b1 104
tetsu_0207 0:eed6204ea3b1 105 area = 3;
tetsu_0207 0:eed6204ea3b1 106 }
tetsu_0207 0:eed6204ea3b1 107 //4
tetsu_0207 0:eed6204ea3b1 108 if((formatX > 0 && formatX < 1) && (formatY < 0 && formatY > -1)){
tetsu_0207 0:eed6204ea3b1 109 rightPowerPercent = -1;
tetsu_0207 0:eed6204ea3b1 110 leftPowerPercent = ((angle / 45) + 1) / 1;
tetsu_0207 0:eed6204ea3b1 111
tetsu_0207 0:eed6204ea3b1 112 area = 4;
tetsu_0207 0:eed6204ea3b1 113 }
tetsu_0207 0:eed6204ea3b1 114 if(area == 0){
tetsu_0207 0:eed6204ea3b1 115 rightPowerPercent = 0;
tetsu_0207 0:eed6204ea3b1 116 leftPowerPercent = 0;
tetsu_0207 0:eed6204ea3b1 117 }
tetsu_0207 0:eed6204ea3b1 118
tetsu_0207 0:eed6204ea3b1 119 // convert power persent to real power
tetsu_0207 0:eed6204ea3b1 120 powerL = allPower * leftPowerPercent;
tetsu_0207 0:eed6204ea3b1 121 powerR = allPower * rightPowerPercent;
tetsu_0207 0:eed6204ea3b1 122
tetsu_0207 0:eed6204ea3b1 123 // need to change power(power distance)
tetsu_0207 0:eed6204ea3b1 124 double needChangePowerL = powerL - currentPowerL;
tetsu_0207 0:eed6204ea3b1 125 double needChangePowerR = powerR - currentPowerR;
tetsu_0207 0:eed6204ea3b1 126 // change power
tetsu_0207 0:eed6204ea3b1 127 double changePowerL = needChangePowerL / POWER_CHANGE_PER_CLOCK;
tetsu_0207 0:eed6204ea3b1 128 double changePowerR = needChangePowerR / POWER_CHANGE_PER_CLOCK;
tetsu_0207 0:eed6204ea3b1 129 // set power
tetsu_0207 0:eed6204ea3b1 130 powerL = currentPowerL + changePowerL;
tetsu_0207 0:eed6204ea3b1 131 powerR = currentPowerR + changePowerR;
tetsu_0207 0:eed6204ea3b1 132 // save current power
tetsu_0207 0:eed6204ea3b1 133 currentPowerL = powerL;
tetsu_0207 0:eed6204ea3b1 134 currentPowerR = powerR;
tetsu_0207 0:eed6204ea3b1 135
tetsu_0207 0:eed6204ea3b1 136 //printf("before:%lf %lf distance:%lf %lf change:%lf %lf after:%lf %lf \n\r",powerL,powerR,needChangePowerL,needChangePowerR,changePowerL,changePowerR,powerL,powerR);
tetsu_0207 0:eed6204ea3b1 137
tetsu_0207 0:eed6204ea3b1 138 if(powerL >= 0){
tetsu_0207 0:eed6204ea3b1 139 dirL = DIR_DEFAULT_L;
tetsu_0207 0:eed6204ea3b1 140 }else{
tetsu_0207 0:eed6204ea3b1 141 powerL = -powerL;
tetsu_0207 0:eed6204ea3b1 142 dirL = !DIR_DEFAULT_L;
tetsu_0207 0:eed6204ea3b1 143 }
tetsu_0207 0:eed6204ea3b1 144 if(powerR >= 0){
tetsu_0207 0:eed6204ea3b1 145 dirR = DIR_DEFAULT_R;
tetsu_0207 0:eed6204ea3b1 146 }else{
tetsu_0207 0:eed6204ea3b1 147 powerR = -powerR;
tetsu_0207 0:eed6204ea3b1 148 dirR = !DIR_DEFAULT_R;
tetsu_0207 0:eed6204ea3b1 149 }
tetsu_0207 0:eed6204ea3b1 150
tetsu_0207 0:eed6204ea3b1 151 pwmL.write((float)powerL);
tetsu_0207 0:eed6204ea3b1 152 pwmR.write((float)powerR);
tetsu_0207 0:eed6204ea3b1 153
tetsu_0207 0:eed6204ea3b1 154 wait_ms(50);
tetsu_0207 0:eed6204ea3b1 155 //printf("powerL:%f powerR:%f formatX:%0lf formatY:%0lf angle:%d range:%0lf area: %d Lper:%0lf Rper:%0lf \n\r",powerL,powerR,formatX,formatY,(int) angle,range,area,leftPowerPercent,rightPowerPercent);
tetsu_0207 0:eed6204ea3b1 156 }
tetsu_0207 0:eed6204ea3b1 157 }
tetsu_0207 0:eed6204ea3b1 158