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@4:679bf698903e, 2020-11-16 (annotated)

Committer:
tetsu_0207
Date:
Mon Nov 16 09:08:56 2020 +0000
Revision:
4:679bf698903e
Parent:
3:0920442e3f03
Child:
5:aef6f39b9683
improve lag

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tetsu_0207 0:eed6204ea3b1 1 #include "mbed.h"
tetsu_0207 3:0920442e3f03 2 #include <vector>
tetsu_0207 0:eed6204ea3b1 3
tetsu_0207 0:eed6204ea3b1 4 // Left Motor
tetsu_0207 0:eed6204ea3b1 5 #define PWM_L A6
tetsu_0207 0:eed6204ea3b1 6 #define DIR_L A3
tetsu_0207 0:eed6204ea3b1 7 // Right Motor
tetsu_0207 0:eed6204ea3b1 8 #define PWM_R A2
tetsu_0207 0:eed6204ea3b1 9 #define DIR_R D12
tetsu_0207 0:eed6204ea3b1 10 // default setting
tetsu_0207 0:eed6204ea3b1 11 #define DIR_DEFAULT_L 1
tetsu_0207 0:eed6204ea3b1 12 #define DIR_DEFAULT_R 0
tetsu_0207 0:eed6204ea3b1 13
tetsu_0207 0:eed6204ea3b1 14 // Math Function
tetsu_0207 0:eed6204ea3b1 15 #define PI 3.14159265359
tetsu_0207 0:eed6204ea3b1 16 // power clock
tetsu_0207 0:eed6204ea3b1 17 #define POWER_CHANGE_PER_CLOCK 5
tetsu_0207 0:eed6204ea3b1 18
tetsu_0207 0:eed6204ea3b1 19 // PWM
tetsu_0207 0:eed6204ea3b1 20 PwmOut pwmL(PWM_L);
tetsu_0207 0:eed6204ea3b1 21 // DIR
tetsu_0207 0:eed6204ea3b1 22 DigitalOut dirL(DIR_L);
tetsu_0207 0:eed6204ea3b1 23 // PWM
tetsu_0207 0:eed6204ea3b1 24 PwmOut pwmR(PWM_R);
tetsu_0207 0:eed6204ea3b1 25 // DIR
tetsu_0207 0:eed6204ea3b1 26 DigitalOut dirR(DIR_R);
tetsu_0207 2:becb78cfc927 27
tetsu_0207 2:becb78cfc927 28 // serial
tetsu_0207 2:becb78cfc927 29 Serial serial(D5,D4);
tetsu_0207 2:becb78cfc927 30
tetsu_0207 0:eed6204ea3b1 31 // joystick analogin
tetsu_0207 2:becb78cfc927 32 //AnalogIn joyX(A0);
tetsu_0207 2:becb78cfc927 33 //AnalogIn joyY(A1);
tetsu_0207 2:becb78cfc927 34 // stick value
tetsu_0207 2:becb78cfc927 35 float x = 0.5F;
tetsu_0207 2:becb78cfc927 36 float y = 0.5F;
tetsu_0207 2:becb78cfc927 37
tetsu_0207 0:eed6204ea3b1 38
tetsu_0207 0:eed6204ea3b1 39 // current power
tetsu_0207 0:eed6204ea3b1 40 double currentPowerL = 0;
tetsu_0207 0:eed6204ea3b1 41 double currentPowerR = 0;
tetsu_0207 0:eed6204ea3b1 42
tetsu_0207 2:becb78cfc927 43 // attach function
tetsu_0207 2:becb78cfc927 44 void control_rx();
tetsu_0207 2:becb78cfc927 45
tetsu_0207 0:eed6204ea3b1 46 int main()
tetsu_0207 0:eed6204ea3b1 47 {
tetsu_0207 0:eed6204ea3b1 48 //printf("start program... \n\r");
tetsu_0207 0:eed6204ea3b1 49 // period
tetsu_0207 0:eed6204ea3b1 50 pwmL.period_us(100);
tetsu_0207 0:eed6204ea3b1 51 pwmR.period_us(100);
tetsu_0207 0:eed6204ea3b1 52 // Dir
tetsu_0207 0:eed6204ea3b1 53 dirL = DIR_DEFAULT_L;
tetsu_0207 0:eed6204ea3b1 54 dirR = DIR_DEFAULT_R;
tetsu_0207 0:eed6204ea3b1 55 // power setting
tetsu_0207 4:679bf698903e 56 float maxPower = 0.97F;
tetsu_0207 3:0920442e3f03 57
tetsu_0207 2:becb78cfc927 58 // serial attach
tetsu_0207 3:0920442e3f03 59 serial.baud(115200);
tetsu_0207 2:becb78cfc927 60 serial.attach(control_rx,Serial::RxIrq);
tetsu_0207 3:0920442e3f03 61
tetsu_0207 0:eed6204ea3b1 62 //printf("finish start up! \n\r");
tetsu_0207 0:eed6204ea3b1 63
tetsu_0207 3:0920442e3f03 64 while(1) {
tetsu_0207 0:eed6204ea3b1 65 // prepare power value
tetsu_0207 0:eed6204ea3b1 66 float powerL,powerR;
tetsu_0207 3:0920442e3f03 67
tetsu_0207 2:becb78cfc927 68 //float x = joyX.read(); // go ahead & back
tetsu_0207 2:becb78cfc927 69 //float y = joyY.read(); // left & right
tetsu_0207 0:eed6204ea3b1 70 // value format
tetsu_0207 0:eed6204ea3b1 71 if(x > 1) x = 1.0;
tetsu_0207 0:eed6204ea3b1 72 if(x < 0) x = 0.0;
tetsu_0207 0:eed6204ea3b1 73 if(y > 1) y = 1.0;
tetsu_0207 0:eed6204ea3b1 74 if(y < 0) y = 0.0;
tetsu_0207 0:eed6204ea3b1 75 // off set
tetsu_0207 0:eed6204ea3b1 76 //if(0.48F < x && x < 0.52F)x = 0.5F;
tetsu_0207 0:eed6204ea3b1 77 //if(0.48F < y && y < 0.52F)y = 0.5F;
tetsu_0207 3:0920442e3f03 78
tetsu_0207 0:eed6204ea3b1 79 // format x and y
tetsu_0207 0:eed6204ea3b1 80 double formatX = (2*x) - 1;
tetsu_0207 0:eed6204ea3b1 81 double formatY = (2*y) - 1;
tetsu_0207 0:eed6204ea3b1 82 // tan
tetsu_0207 0:eed6204ea3b1 83 double tan = formatY / formatX;
tetsu_0207 0:eed6204ea3b1 84 // arc tan
tetsu_0207 0:eed6204ea3b1 85 double arctan = (double) atan(tan);
tetsu_0207 0:eed6204ea3b1 86 // angle
tetsu_0207 0:eed6204ea3b1 87 double angle = arctan * (180 / PI);
tetsu_0207 0:eed6204ea3b1 88 // range
tetsu_0207 0:eed6204ea3b1 89 double range = sqrt((formatX * formatX) + (formatY * formatY));
tetsu_0207 0:eed6204ea3b1 90 if(range > 1.0F) range = 1.0F;
tetsu_0207 0:eed6204ea3b1 91 // all power
tetsu_0207 0:eed6204ea3b1 92 double allPower = maxPower * (range / 1);
tetsu_0207 3:0920442e3f03 93
tetsu_0207 0:eed6204ea3b1 94 // right left power persent
tetsu_0207 0:eed6204ea3b1 95 double rightPowerPercent,leftPowerPercent;
tetsu_0207 0:eed6204ea3b1 96 int area = 0;
tetsu_0207 0:eed6204ea3b1 97 //1
tetsu_0207 3:0920442e3f03 98 if((0 < formatX && formatX < 1) && (0 < formatY && formatY < 1)) {
tetsu_0207 0:eed6204ea3b1 99 rightPowerPercent = ((angle / 45) - 1) / 1;
tetsu_0207 0:eed6204ea3b1 100 leftPowerPercent = 1;
tetsu_0207 3:0920442e3f03 101
tetsu_0207 0:eed6204ea3b1 102 area = 1;
tetsu_0207 0:eed6204ea3b1 103 }
tetsu_0207 0:eed6204ea3b1 104 //2
tetsu_0207 3:0920442e3f03 105 if((formatX < 0 && -1 < formatX) && (0 < formatY && formatY < 1)) {
tetsu_0207 0:eed6204ea3b1 106 rightPowerPercent = 1;
tetsu_0207 0:eed6204ea3b1 107 leftPowerPercent = ((-angle / 45) - 1) / 1;
tetsu_0207 3:0920442e3f03 108
tetsu_0207 0:eed6204ea3b1 109 area = 2;
tetsu_0207 0:eed6204ea3b1 110 }
tetsu_0207 0:eed6204ea3b1 111 //3
tetsu_0207 3:0920442e3f03 112 if((formatX < 0 && -1 < formatX) && (formatY < 0 && -1 < formatY)) {
tetsu_0207 0:eed6204ea3b1 113 rightPowerPercent = ((-angle / 45) + 1) / 1;
tetsu_0207 0:eed6204ea3b1 114 leftPowerPercent = -1;
tetsu_0207 3:0920442e3f03 115
tetsu_0207 0:eed6204ea3b1 116 area = 3;
tetsu_0207 0:eed6204ea3b1 117 }
tetsu_0207 0:eed6204ea3b1 118 //4
tetsu_0207 3:0920442e3f03 119 if((formatX > 0 && formatX < 1) && (formatY < 0 && formatY > -1)) {
tetsu_0207 0:eed6204ea3b1 120 rightPowerPercent = -1;
tetsu_0207 0:eed6204ea3b1 121 leftPowerPercent = ((angle / 45) + 1) / 1;
tetsu_0207 3:0920442e3f03 122
tetsu_0207 0:eed6204ea3b1 123 area = 4;
tetsu_0207 0:eed6204ea3b1 124 }
tetsu_0207 3:0920442e3f03 125 if(area == 0) {
tetsu_0207 0:eed6204ea3b1 126 rightPowerPercent = 0;
tetsu_0207 0:eed6204ea3b1 127 leftPowerPercent = 0;
tetsu_0207 0:eed6204ea3b1 128 }
tetsu_0207 3:0920442e3f03 129
tetsu_0207 0:eed6204ea3b1 130 // convert power persent to real power
tetsu_0207 0:eed6204ea3b1 131 powerL = allPower * leftPowerPercent;
tetsu_0207 0:eed6204ea3b1 132 powerR = allPower * rightPowerPercent;
tetsu_0207 3:0920442e3f03 133
tetsu_0207 0:eed6204ea3b1 134 // need to change power(power distance)
tetsu_0207 0:eed6204ea3b1 135 double needChangePowerL = powerL - currentPowerL;
tetsu_0207 0:eed6204ea3b1 136 double needChangePowerR = powerR - currentPowerR;
tetsu_0207 0:eed6204ea3b1 137 // change power
tetsu_0207 0:eed6204ea3b1 138 double changePowerL = needChangePowerL / POWER_CHANGE_PER_CLOCK;
tetsu_0207 0:eed6204ea3b1 139 double changePowerR = needChangePowerR / POWER_CHANGE_PER_CLOCK;
tetsu_0207 0:eed6204ea3b1 140 // set power
tetsu_0207 0:eed6204ea3b1 141 powerL = currentPowerL + changePowerL;
tetsu_0207 0:eed6204ea3b1 142 powerR = currentPowerR + changePowerR;
tetsu_0207 0:eed6204ea3b1 143 // save current power
tetsu_0207 0:eed6204ea3b1 144 currentPowerL = powerL;
tetsu_0207 0:eed6204ea3b1 145 currentPowerR = powerR;
tetsu_0207 3:0920442e3f03 146
tetsu_0207 0:eed6204ea3b1 147 //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 3:0920442e3f03 148
tetsu_0207 3:0920442e3f03 149 if(powerL >= 0) {
tetsu_0207 3:0920442e3f03 150 dirL = DIR_DEFAULT_L;
tetsu_0207 3:0920442e3f03 151 } else {
tetsu_0207 0:eed6204ea3b1 152 powerL = -powerL;
tetsu_0207 0:eed6204ea3b1 153 dirL = !DIR_DEFAULT_L;
tetsu_0207 0:eed6204ea3b1 154 }
tetsu_0207 3:0920442e3f03 155 if(powerR >= 0) {
tetsu_0207 3:0920442e3f03 156 dirR = DIR_DEFAULT_R;
tetsu_0207 3:0920442e3f03 157 } else {
tetsu_0207 0:eed6204ea3b1 158 powerR = -powerR;
tetsu_0207 0:eed6204ea3b1 159 dirR = !DIR_DEFAULT_R;
tetsu_0207 0:eed6204ea3b1 160 }
tetsu_0207 3:0920442e3f03 161
tetsu_0207 0:eed6204ea3b1 162 pwmL.write((float)powerL);
tetsu_0207 0:eed6204ea3b1 163 pwmR.write((float)powerR);
tetsu_0207 3:0920442e3f03 164
tetsu_0207 0:eed6204ea3b1 165 wait_ms(50);
tetsu_0207 0:eed6204ea3b1 166 //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 167 }
tetsu_0207 0:eed6204ea3b1 168 }
tetsu_0207 0:eed6204ea3b1 169
tetsu_0207 3:0920442e3f03 170
tetsu_0207 3:0920442e3f03 171
tetsu_0207 4:679bf698903e 172 int head;
tetsu_0207 3:0920442e3f03 173 char bytes[5];
tetsu_0207 3:0920442e3f03 174
tetsu_0207 3:0920442e3f03 175 void control_rx()
tetsu_0207 3:0920442e3f03 176 {
tetsu_0207 4:679bf698903e 177
tetsu_0207 3:0920442e3f03 178 while(serial.readable()) {
tetsu_0207 2:becb78cfc927 179 char c = serial.getc();
tetsu_0207 3:0920442e3f03 180 if(c == 0x3A) {
tetsu_0207 2:becb78cfc927 181 head = 0;
tetsu_0207 2:becb78cfc927 182 }
tetsu_0207 3:0920442e3f03 183 if(head < 5 && head != -1) {
tetsu_0207 3:0920442e3f03 184 bytes[head] = c;
tetsu_0207 3:0920442e3f03 185 head++;
tetsu_0207 3:0920442e3f03 186 } else {
tetsu_0207 3:0920442e3f03 187 printf("error: failed get data[head:%d]\n\r",head);
tetsu_0207 3:0920442e3f03 188 head = -1;
tetsu_0207 3:0920442e3f03 189 break;
tetsu_0207 2:becb78cfc927 190 }
tetsu_0207 3:0920442e3f03 191 if(head == 5) {
tetsu_0207 3:0920442e3f03 192 if(bytes[0] == 0x3A) {
tetsu_0207 3:0920442e3f03 193 // cast float to double
tetsu_0207 4:679bf698903e 194 uint16_t uintX = ((bytes[1] << 8 | bytes[2]) & 0xffff);
tetsu_0207 4:679bf698903e 195 uint16_t uintY = ((bytes[3] << 8 | bytes[4]) & 0xffff);
tetsu_0207 4:679bf698903e 196 double formatX = (double)uintX / 65536;
tetsu_0207 4:679bf698903e 197 double formatY = (double)uintY / 65536;
tetsu_0207 3:0920442e3f03 198 // save
tetsu_0207 3:0920442e3f03 199 x = formatX;
tetsu_0207 3:0920442e3f03 200 y = formatY;
tetsu_0207 4:679bf698903e 201 //printf("x:%hu y:%hu byte[%x %x %x %x]\n\r",uintX,uintY,bytes[1],bytes[2],bytes[3],bytes[4]);
tetsu_0207 3:0920442e3f03 202 break;
tetsu_0207 4:679bf698903e 203 }else{
tetsu_0207 4:679bf698903e 204 head = -1;
tetsu_0207 4:679bf698903e 205 break;
tetsu_0207 3:0920442e3f03 206 }
tetsu_0207 2:becb78cfc927 207 }
tetsu_0207 2:becb78cfc927 208 }
tetsu_0207 2:becb78cfc927 209 }