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@6:17e3a28338dc, 2020-11-28 (annotated)

Committer:
tetsu_0207
Date:
Sat Nov 28 07:48:16 2020 +0000
Revision:
6:17e3a28338dc
Parent:
5:aef6f39b9683
Child:
7:0bc2bc07f2fe
[Debug]

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 6:17e3a28338dc 3 // Buffer Serial
tetsu_0207 6:17e3a28338dc 4 #include "BufferSerial.h"
tetsu_0207 0:eed6204ea3b1 5
tetsu_0207 0:eed6204ea3b1 6 // Left Motor
tetsu_0207 0:eed6204ea3b1 7 #define PWM_L A6
tetsu_0207 0:eed6204ea3b1 8 #define DIR_L A3
tetsu_0207 0:eed6204ea3b1 9 // Right Motor
tetsu_0207 0:eed6204ea3b1 10 #define PWM_R A2
tetsu_0207 0:eed6204ea3b1 11 #define DIR_R D12
tetsu_0207 0:eed6204ea3b1 12 // default setting
tetsu_0207 0:eed6204ea3b1 13 #define DIR_DEFAULT_L 1
tetsu_0207 0:eed6204ea3b1 14 #define DIR_DEFAULT_R 0
tetsu_0207 0:eed6204ea3b1 15
tetsu_0207 0:eed6204ea3b1 16 // Math Function
tetsu_0207 0:eed6204ea3b1 17 #define PI 3.14159265359
tetsu_0207 0:eed6204ea3b1 18 // power clock
tetsu_0207 0:eed6204ea3b1 19 #define POWER_CHANGE_PER_CLOCK 5
tetsu_0207 0:eed6204ea3b1 20
tetsu_0207 5:aef6f39b9683 21 // connect timeout LED
tetsu_0207 5:aef6f39b9683 22 #define CONNECT_TIMEOUT_LED D2
tetsu_0207 5:aef6f39b9683 23
tetsu_0207 0:eed6204ea3b1 24 // PWM
tetsu_0207 0:eed6204ea3b1 25 PwmOut pwmL(PWM_L);
tetsu_0207 0:eed6204ea3b1 26 // DIR
tetsu_0207 0:eed6204ea3b1 27 DigitalOut dirL(DIR_L);
tetsu_0207 0:eed6204ea3b1 28 // PWM
tetsu_0207 0:eed6204ea3b1 29 PwmOut pwmR(PWM_R);
tetsu_0207 0:eed6204ea3b1 30 // DIR
tetsu_0207 0:eed6204ea3b1 31 DigitalOut dirR(DIR_R);
tetsu_0207 2:becb78cfc927 32
tetsu_0207 2:becb78cfc927 33 // serial
tetsu_0207 6:17e3a28338dc 34 //Serial serial(D5,D4);
tetsu_0207 6:17e3a28338dc 35 // bufferSerial
tetsu_0207 6:17e3a28338dc 36 BufferSerial _buffer_serial(D5,D4,115200,2000);
tetsu_0207 6:17e3a28338dc 37
tetsu_0207 5:aef6f39b9683 38 // time out led
tetsu_0207 5:aef6f39b9683 39 DigitalOut connectTimeoutLED(CONNECT_TIMEOUT_LED);
tetsu_0207 2:becb78cfc927 40
tetsu_0207 0:eed6204ea3b1 41 // joystick analogin
tetsu_0207 2:becb78cfc927 42 //AnalogIn joyX(A0);
tetsu_0207 2:becb78cfc927 43 //AnalogIn joyY(A1);
tetsu_0207 2:becb78cfc927 44 // stick value
tetsu_0207 2:becb78cfc927 45 float x = 0.5F;
tetsu_0207 2:becb78cfc927 46 float y = 0.5F;
tetsu_0207 2:becb78cfc927 47
tetsu_0207 0:eed6204ea3b1 48
tetsu_0207 0:eed6204ea3b1 49 // current power
tetsu_0207 0:eed6204ea3b1 50 double currentPowerL = 0;
tetsu_0207 0:eed6204ea3b1 51 double currentPowerR = 0;
tetsu_0207 0:eed6204ea3b1 52
tetsu_0207 2:becb78cfc927 53 // attach function
tetsu_0207 2:becb78cfc927 54 void control_rx();
tetsu_0207 5:aef6f39b9683 55 // time out
tetsu_0207 5:aef6f39b9683 56 Timer connectTimer;
tetsu_0207 5:aef6f39b9683 57 int connectLimit = 2000;
tetsu_0207 2:becb78cfc927 58
tetsu_0207 6:17e3a28338dc 59 void load_data();
tetsu_0207 6:17e3a28338dc 60
tetsu_0207 0:eed6204ea3b1 61 int main()
tetsu_0207 0:eed6204ea3b1 62 {
tetsu_0207 0:eed6204ea3b1 63 // period
tetsu_0207 0:eed6204ea3b1 64 pwmL.period_us(100);
tetsu_0207 0:eed6204ea3b1 65 pwmR.period_us(100);
tetsu_0207 0:eed6204ea3b1 66 // Dir
tetsu_0207 0:eed6204ea3b1 67 dirL = DIR_DEFAULT_L;
tetsu_0207 0:eed6204ea3b1 68 dirR = DIR_DEFAULT_R;
tetsu_0207 0:eed6204ea3b1 69 // power setting
tetsu_0207 6:17e3a28338dc 70 float maxPower = 0.95F;
tetsu_0207 3:0920442e3f03 71
tetsu_0207 6:17e3a28338dc 72 /*
tetsu_0207 2:becb78cfc927 73 // serial attach
tetsu_0207 3:0920442e3f03 74 serial.baud(115200);
tetsu_0207 2:becb78cfc927 75 serial.attach(control_rx,Serial::RxIrq);
tetsu_0207 6:17e3a28338dc 76 */
tetsu_0207 5:aef6f39b9683 77 // connect timer
tetsu_0207 5:aef6f39b9683 78 connectTimer.reset();
tetsu_0207 5:aef6f39b9683 79 connectTimer.start();
tetsu_0207 6:17e3a28338dc 80
tetsu_0207 3:0920442e3f03 81 while(1) {
tetsu_0207 0:eed6204ea3b1 82 // prepare power value
tetsu_0207 0:eed6204ea3b1 83 float powerL,powerR;
tetsu_0207 3:0920442e3f03 84
tetsu_0207 6:17e3a28338dc 85 // loat data
tetsu_0207 6:17e3a28338dc 86 load_data();
tetsu_0207 6:17e3a28338dc 87
tetsu_0207 5:aef6f39b9683 88 // last connect checker
tetsu_0207 6:17e3a28338dc 89 if(connectTimer.read_ms() > connectLimit) {
tetsu_0207 5:aef6f39b9683 90 x = 0.5;
tetsu_0207 5:aef6f39b9683 91 y = 0.5;
tetsu_0207 5:aef6f39b9683 92 // LED
tetsu_0207 5:aef6f39b9683 93 connectTimeoutLED = !connectTimeoutLED;
tetsu_0207 5:aef6f39b9683 94 wait_ms(50);
tetsu_0207 5:aef6f39b9683 95 }
tetsu_0207 0:eed6204ea3b1 96 // value format
tetsu_0207 0:eed6204ea3b1 97 if(x > 1) x = 1.0;
tetsu_0207 0:eed6204ea3b1 98 if(x < 0) x = 0.0;
tetsu_0207 0:eed6204ea3b1 99 if(y > 1) y = 1.0;
tetsu_0207 0:eed6204ea3b1 100 if(y < 0) y = 0.0;
tetsu_0207 3:0920442e3f03 101
tetsu_0207 0:eed6204ea3b1 102 // format x and y
tetsu_0207 0:eed6204ea3b1 103 double formatX = (2*x) - 1;
tetsu_0207 0:eed6204ea3b1 104 double formatY = (2*y) - 1;
tetsu_0207 0:eed6204ea3b1 105 // tan
tetsu_0207 0:eed6204ea3b1 106 double tan = formatY / formatX;
tetsu_0207 0:eed6204ea3b1 107 // arc tan
tetsu_0207 0:eed6204ea3b1 108 double arctan = (double) atan(tan);
tetsu_0207 0:eed6204ea3b1 109 // angle
tetsu_0207 0:eed6204ea3b1 110 double angle = arctan * (180 / PI);
tetsu_0207 0:eed6204ea3b1 111 // range
tetsu_0207 0:eed6204ea3b1 112 double range = sqrt((formatX * formatX) + (formatY * formatY));
tetsu_0207 0:eed6204ea3b1 113 if(range > 1.0F) range = 1.0F;
tetsu_0207 0:eed6204ea3b1 114 // all power
tetsu_0207 0:eed6204ea3b1 115 double allPower = maxPower * (range / 1);
tetsu_0207 3:0920442e3f03 116
tetsu_0207 0:eed6204ea3b1 117 // right left power persent
tetsu_0207 0:eed6204ea3b1 118 double rightPowerPercent,leftPowerPercent;
tetsu_0207 0:eed6204ea3b1 119 int area = 0;
tetsu_0207 0:eed6204ea3b1 120 //1
tetsu_0207 3:0920442e3f03 121 if((0 < formatX && formatX < 1) && (0 < formatY && formatY < 1)) {
tetsu_0207 0:eed6204ea3b1 122 rightPowerPercent = ((angle / 45) - 1) / 1;
tetsu_0207 0:eed6204ea3b1 123 leftPowerPercent = 1;
tetsu_0207 3:0920442e3f03 124
tetsu_0207 0:eed6204ea3b1 125 area = 1;
tetsu_0207 0:eed6204ea3b1 126 }
tetsu_0207 0:eed6204ea3b1 127 //2
tetsu_0207 3:0920442e3f03 128 if((formatX < 0 && -1 < formatX) && (0 < formatY && formatY < 1)) {
tetsu_0207 0:eed6204ea3b1 129 rightPowerPercent = 1;
tetsu_0207 0:eed6204ea3b1 130 leftPowerPercent = ((-angle / 45) - 1) / 1;
tetsu_0207 3:0920442e3f03 131
tetsu_0207 0:eed6204ea3b1 132 area = 2;
tetsu_0207 0:eed6204ea3b1 133 }
tetsu_0207 0:eed6204ea3b1 134 //3
tetsu_0207 3:0920442e3f03 135 if((formatX < 0 && -1 < formatX) && (formatY < 0 && -1 < formatY)) {
tetsu_0207 0:eed6204ea3b1 136 rightPowerPercent = ((-angle / 45) + 1) / 1;
tetsu_0207 0:eed6204ea3b1 137 leftPowerPercent = -1;
tetsu_0207 3:0920442e3f03 138
tetsu_0207 0:eed6204ea3b1 139 area = 3;
tetsu_0207 0:eed6204ea3b1 140 }
tetsu_0207 0:eed6204ea3b1 141 //4
tetsu_0207 3:0920442e3f03 142 if((formatX > 0 && formatX < 1) && (formatY < 0 && formatY > -1)) {
tetsu_0207 0:eed6204ea3b1 143 rightPowerPercent = -1;
tetsu_0207 0:eed6204ea3b1 144 leftPowerPercent = ((angle / 45) + 1) / 1;
tetsu_0207 3:0920442e3f03 145
tetsu_0207 0:eed6204ea3b1 146 area = 4;
tetsu_0207 0:eed6204ea3b1 147 }
tetsu_0207 3:0920442e3f03 148 if(area == 0) {
tetsu_0207 0:eed6204ea3b1 149 rightPowerPercent = 0;
tetsu_0207 0:eed6204ea3b1 150 leftPowerPercent = 0;
tetsu_0207 0:eed6204ea3b1 151 }
tetsu_0207 3:0920442e3f03 152
tetsu_0207 0:eed6204ea3b1 153 // convert power persent to real power
tetsu_0207 0:eed6204ea3b1 154 powerL = allPower * leftPowerPercent;
tetsu_0207 0:eed6204ea3b1 155 powerR = allPower * rightPowerPercent;
tetsu_0207 3:0920442e3f03 156
tetsu_0207 0:eed6204ea3b1 157 // need to change power(power distance)
tetsu_0207 0:eed6204ea3b1 158 double needChangePowerL = powerL - currentPowerL;
tetsu_0207 0:eed6204ea3b1 159 double needChangePowerR = powerR - currentPowerR;
tetsu_0207 0:eed6204ea3b1 160 // change power
tetsu_0207 0:eed6204ea3b1 161 double changePowerL = needChangePowerL / POWER_CHANGE_PER_CLOCK;
tetsu_0207 0:eed6204ea3b1 162 double changePowerR = needChangePowerR / POWER_CHANGE_PER_CLOCK;
tetsu_0207 0:eed6204ea3b1 163 // set power
tetsu_0207 0:eed6204ea3b1 164 powerL = currentPowerL + changePowerL;
tetsu_0207 0:eed6204ea3b1 165 powerR = currentPowerR + changePowerR;
tetsu_0207 0:eed6204ea3b1 166 // save current power
tetsu_0207 0:eed6204ea3b1 167 currentPowerL = powerL;
tetsu_0207 0:eed6204ea3b1 168 currentPowerR = powerR;
tetsu_0207 3:0920442e3f03 169
tetsu_0207 0:eed6204ea3b1 170 //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 171
tetsu_0207 3:0920442e3f03 172 if(powerL >= 0) {
tetsu_0207 3:0920442e3f03 173 dirL = DIR_DEFAULT_L;
tetsu_0207 3:0920442e3f03 174 } else {
tetsu_0207 0:eed6204ea3b1 175 powerL = -powerL;
tetsu_0207 0:eed6204ea3b1 176 dirL = !DIR_DEFAULT_L;
tetsu_0207 0:eed6204ea3b1 177 }
tetsu_0207 3:0920442e3f03 178 if(powerR >= 0) {
tetsu_0207 3:0920442e3f03 179 dirR = DIR_DEFAULT_R;
tetsu_0207 3:0920442e3f03 180 } else {
tetsu_0207 0:eed6204ea3b1 181 powerR = -powerR;
tetsu_0207 0:eed6204ea3b1 182 dirR = !DIR_DEFAULT_R;
tetsu_0207 0:eed6204ea3b1 183 }
tetsu_0207 3:0920442e3f03 184
tetsu_0207 0:eed6204ea3b1 185 pwmL.write((float)powerL);
tetsu_0207 0:eed6204ea3b1 186 pwmR.write((float)powerR);
tetsu_0207 3:0920442e3f03 187
tetsu_0207 0:eed6204ea3b1 188 wait_ms(50);
tetsu_0207 0:eed6204ea3b1 189 //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 190 }
tetsu_0207 0:eed6204ea3b1 191 }
tetsu_0207 0:eed6204ea3b1 192
tetsu_0207 3:0920442e3f03 193
tetsu_0207 6:17e3a28338dc 194 /*
tetsu_0207 4:679bf698903e 195 int head;
tetsu_0207 3:0920442e3f03 196 char bytes[5];
tetsu_0207 3:0920442e3f03 197
tetsu_0207 3:0920442e3f03 198 void control_rx()
tetsu_0207 3:0920442e3f03 199 {
tetsu_0207 6:17e3a28338dc 200
tetsu_0207 3:0920442e3f03 201 while(serial.readable()) {
tetsu_0207 2:becb78cfc927 202 char c = serial.getc();
tetsu_0207 3:0920442e3f03 203 if(c == 0x3A) {
tetsu_0207 2:becb78cfc927 204 head = 0;
tetsu_0207 2:becb78cfc927 205 }
tetsu_0207 3:0920442e3f03 206 if(head < 5 && head != -1) {
tetsu_0207 3:0920442e3f03 207 bytes[head] = c;
tetsu_0207 3:0920442e3f03 208 head++;
tetsu_0207 3:0920442e3f03 209 } else {
tetsu_0207 3:0920442e3f03 210 printf("error: failed get data[head:%d]\n\r",head);
tetsu_0207 3:0920442e3f03 211 head = -1;
tetsu_0207 3:0920442e3f03 212 break;
tetsu_0207 2:becb78cfc927 213 }
tetsu_0207 3:0920442e3f03 214 if(head == 5) {
tetsu_0207 3:0920442e3f03 215 if(bytes[0] == 0x3A) {
tetsu_0207 3:0920442e3f03 216 // cast float to double
tetsu_0207 4:679bf698903e 217 uint16_t uintX = ((bytes[1] << 8 | bytes[2]) & 0xffff);
tetsu_0207 4:679bf698903e 218 uint16_t uintY = ((bytes[3] << 8 | bytes[4]) & 0xffff);
tetsu_0207 4:679bf698903e 219 double formatX = (double)uintX / 65536;
tetsu_0207 4:679bf698903e 220 double formatY = (double)uintY / 65536;
tetsu_0207 3:0920442e3f03 221 // save
tetsu_0207 3:0920442e3f03 222 x = formatX;
tetsu_0207 3:0920442e3f03 223 y = formatY;
tetsu_0207 6:17e3a28338dc 224
tetsu_0207 5:aef6f39b9683 225 // reset connect timer
tetsu_0207 5:aef6f39b9683 226 connectTimer.reset();
tetsu_0207 3:0920442e3f03 227 break;
tetsu_0207 4:679bf698903e 228 }else{
tetsu_0207 4:679bf698903e 229 head = -1;
tetsu_0207 6:17e3a28338dc 230 break;
tetsu_0207 3:0920442e3f03 231 }
tetsu_0207 2:becb78cfc927 232 }
tetsu_0207 2:becb78cfc927 233 }
tetsu_0207 6:17e3a28338dc 234 }*/
tetsu_0207 6:17e3a28338dc 235
tetsu_0207 6:17e3a28338dc 236 void load_data()
tetsu_0207 6:17e3a28338dc 237 {
tetsu_0207 6:17e3a28338dc 238 /*
tetsu_0207 6:17e3a28338dc 239 // buffer check
tetsu_0207 6:17e3a28338dc 240 int buffer_length = sizeof(_buffer_serial._rx_buffer) / sizeof(_buffer_serial._rx_buffer[0]);
tetsu_0207 6:17e3a28338dc 241 for(int i = 0; i <= buffer_length; i++) {
tetsu_0207 6:17e3a28338dc 242 printf("%x ",_buffer_serial._rx_buffer[i]);
tetsu_0207 6:17e3a28338dc 243 }
tetsu_0207 6:17e3a28338dc 244 printf("\n\r");
tetsu_0207 6:17e3a28338dc 245 */
tetsu_0207 6:17e3a28338dc 246 char bytes[6];
tetsu_0207 6:17e3a28338dc 247 size_t index = _buffer_serial.readBytesUntil('\n',(char*)bytes,6);
tetsu_0207 6:17e3a28338dc 248 if(index != 5){
tetsu_0207 6:17e3a28338dc 249 //printf("e:index %d\n\r",index);
tetsu_0207 6:17e3a28338dc 250 return;
tetsu_0207 6:17e3a28338dc 251 }
tetsu_0207 6:17e3a28338dc 252 if(bytes[0] == 0x3A) {
tetsu_0207 6:17e3a28338dc 253 // cast float to double
tetsu_0207 6:17e3a28338dc 254 uint16_t uintX = ((bytes[1] << 8 | bytes[2]) & 0xffff);
tetsu_0207 6:17e3a28338dc 255 uint16_t uintY = ((bytes[3] << 8 | bytes[4]) & 0xffff);
tetsu_0207 6:17e3a28338dc 256 double formatX = (double)uintX / 65536;
tetsu_0207 6:17e3a28338dc 257 double formatY = (double)uintY / 65536;
tetsu_0207 6:17e3a28338dc 258 // save
tetsu_0207 6:17e3a28338dc 259 x = formatX;
tetsu_0207 6:17e3a28338dc 260 y = formatY;
tetsu_0207 6:17e3a28338dc 261
tetsu_0207 6:17e3a28338dc 262 //printf("%lf %lf\n\r",formatX,formatY);
tetsu_0207 6:17e3a28338dc 263 printf("%x %x %x %x %x %x %lf %lf\n\r",bytes[0],bytes[1],bytes[2],bytes[3],bytes[4],bytes[5],formatX,formatY);
tetsu_0207 6:17e3a28338dc 264 // reset connect timer
tetsu_0207 6:17e3a28338dc 265 connectTimer.reset();
tetsu_0207 6:17e3a28338dc 266
tetsu_0207 6:17e3a28338dc 267 } else {
tetsu_0207 6:17e3a28338dc 268 //printf("failed load_data()\n\r");
tetsu_0207 6:17e3a28338dc 269 }
tetsu_0207 2:becb78cfc927 270 }