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@5:aef6f39b9683, 2020-11-17 (annotated)

Committer:
tetsu_0207
Date:
Tue Nov 17 08:29:41 2020 +0000
Revision:
5:aef6f39b9683
Parent:
4:679bf698903e
Child:
6:17e3a28338dc
[Add] add serial connect time out

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