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@2:becb78cfc927, 2020-11-02 (annotated)

Committer:
tetsu_0207
Date:
Mon Nov 02 11:42:17 2020 +0000
Revision:
2:becb78cfc927
Parent:
1:0ef4f75d84b5
Child:
3:0920442e3f03
[Add] add serial for control

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 2:becb78cfc927 26
tetsu_0207 2:becb78cfc927 27 // serial
tetsu_0207 2:becb78cfc927 28 Serial serial(D5,D4);
tetsu_0207 2:becb78cfc927 29
tetsu_0207 0:eed6204ea3b1 30 // joystick analogin
tetsu_0207 2:becb78cfc927 31 //AnalogIn joyX(A0);
tetsu_0207 2:becb78cfc927 32 //AnalogIn joyY(A1);
tetsu_0207 2:becb78cfc927 33 // stick value
tetsu_0207 2:becb78cfc927 34 float x = 0.5F;
tetsu_0207 2:becb78cfc927 35 float y = 0.5F;
tetsu_0207 2:becb78cfc927 36
tetsu_0207 0:eed6204ea3b1 37
tetsu_0207 0:eed6204ea3b1 38 // current power
tetsu_0207 0:eed6204ea3b1 39 double currentPowerL = 0;
tetsu_0207 0:eed6204ea3b1 40 double currentPowerR = 0;
tetsu_0207 0:eed6204ea3b1 41
tetsu_0207 2:becb78cfc927 42 // attach function
tetsu_0207 2:becb78cfc927 43 void control_rx();
tetsu_0207 2:becb78cfc927 44
tetsu_0207 0:eed6204ea3b1 45 int main()
tetsu_0207 0:eed6204ea3b1 46 {
tetsu_0207 0:eed6204ea3b1 47 //printf("start program... \n\r");
tetsu_0207 0:eed6204ea3b1 48 // period
tetsu_0207 0:eed6204ea3b1 49 pwmL.period_us(100);
tetsu_0207 0:eed6204ea3b1 50 pwmR.period_us(100);
tetsu_0207 0:eed6204ea3b1 51 // Dir
tetsu_0207 0:eed6204ea3b1 52 dirL = DIR_DEFAULT_L;
tetsu_0207 0:eed6204ea3b1 53 dirR = DIR_DEFAULT_R;
tetsu_0207 0:eed6204ea3b1 54 // power setting
tetsu_0207 0:eed6204ea3b1 55 float maxPower = 0.9F;
tetsu_0207 0:eed6204ea3b1 56
tetsu_0207 2:becb78cfc927 57 // serial attach
tetsu_0207 2:becb78cfc927 58 serial.attach(control_rx,Serial::RxIrq);
tetsu_0207 2:becb78cfc927 59
tetsu_0207 0:eed6204ea3b1 60 //printf("finish start up! \n\r");
tetsu_0207 0:eed6204ea3b1 61
tetsu_0207 0:eed6204ea3b1 62 while(1){
tetsu_0207 0:eed6204ea3b1 63 // prepare power value
tetsu_0207 0:eed6204ea3b1 64 float powerL,powerR;
tetsu_0207 0:eed6204ea3b1 65
tetsu_0207 2:becb78cfc927 66 //float x = joyX.read(); // go ahead & back
tetsu_0207 2:becb78cfc927 67 //float y = joyY.read(); // left & right
tetsu_0207 0:eed6204ea3b1 68 // value format
tetsu_0207 0:eed6204ea3b1 69 if(x > 1) x = 1.0;
tetsu_0207 0:eed6204ea3b1 70 if(x < 0) x = 0.0;
tetsu_0207 0:eed6204ea3b1 71 if(y > 1) y = 1.0;
tetsu_0207 0:eed6204ea3b1 72 if(y < 0) y = 0.0;
tetsu_0207 0:eed6204ea3b1 73 // off set
tetsu_0207 0:eed6204ea3b1 74 //if(0.48F < x && x < 0.52F)x = 0.5F;
tetsu_0207 0:eed6204ea3b1 75 //if(0.48F < y && y < 0.52F)y = 0.5F;
tetsu_0207 0:eed6204ea3b1 76
tetsu_0207 0:eed6204ea3b1 77 // format x and y
tetsu_0207 0:eed6204ea3b1 78 double formatX = (2*x) - 1;
tetsu_0207 0:eed6204ea3b1 79 double formatY = (2*y) - 1;
tetsu_0207 0:eed6204ea3b1 80 // tan
tetsu_0207 0:eed6204ea3b1 81 double tan = formatY / formatX;
tetsu_0207 0:eed6204ea3b1 82 // arc tan
tetsu_0207 0:eed6204ea3b1 83 double arctan = (double) atan(tan);
tetsu_0207 0:eed6204ea3b1 84 // angle
tetsu_0207 0:eed6204ea3b1 85 double angle = arctan * (180 / PI);
tetsu_0207 0:eed6204ea3b1 86 // range
tetsu_0207 0:eed6204ea3b1 87 double range = sqrt((formatX * formatX) + (formatY * formatY));
tetsu_0207 0:eed6204ea3b1 88 if(range > 1.0F) range = 1.0F;
tetsu_0207 0:eed6204ea3b1 89 // all power
tetsu_0207 0:eed6204ea3b1 90 double allPower = maxPower * (range / 1);
tetsu_0207 0:eed6204ea3b1 91
tetsu_0207 0:eed6204ea3b1 92 // right left power persent
tetsu_0207 0:eed6204ea3b1 93 double rightPowerPercent,leftPowerPercent;
tetsu_0207 0:eed6204ea3b1 94 int area = 0;
tetsu_0207 0:eed6204ea3b1 95 //1
tetsu_0207 0:eed6204ea3b1 96 if((0 < formatX && formatX < 1) && (0 < formatY && formatY < 1)){
tetsu_0207 0:eed6204ea3b1 97 rightPowerPercent = ((angle / 45) - 1) / 1;
tetsu_0207 0:eed6204ea3b1 98 leftPowerPercent = 1;
tetsu_0207 0:eed6204ea3b1 99
tetsu_0207 0:eed6204ea3b1 100 area = 1;
tetsu_0207 0:eed6204ea3b1 101 }
tetsu_0207 0:eed6204ea3b1 102 //2
tetsu_0207 0:eed6204ea3b1 103 if((formatX < 0 && -1 < formatX) && (0 < formatY && formatY < 1)){
tetsu_0207 0:eed6204ea3b1 104 rightPowerPercent = 1;
tetsu_0207 0:eed6204ea3b1 105 leftPowerPercent = ((-angle / 45) - 1) / 1;
tetsu_0207 0:eed6204ea3b1 106
tetsu_0207 0:eed6204ea3b1 107 area = 2;
tetsu_0207 0:eed6204ea3b1 108 }
tetsu_0207 0:eed6204ea3b1 109 //3
tetsu_0207 0:eed6204ea3b1 110 if((formatX < 0 && -1 < formatX) && (formatY < 0 && -1 < formatY)){
tetsu_0207 0:eed6204ea3b1 111 rightPowerPercent = ((-angle / 45) + 1) / 1;
tetsu_0207 0:eed6204ea3b1 112 leftPowerPercent = -1;
tetsu_0207 0:eed6204ea3b1 113
tetsu_0207 0:eed6204ea3b1 114 area = 3;
tetsu_0207 0:eed6204ea3b1 115 }
tetsu_0207 0:eed6204ea3b1 116 //4
tetsu_0207 0:eed6204ea3b1 117 if((formatX > 0 && formatX < 1) && (formatY < 0 && formatY > -1)){
tetsu_0207 0:eed6204ea3b1 118 rightPowerPercent = -1;
tetsu_0207 0:eed6204ea3b1 119 leftPowerPercent = ((angle / 45) + 1) / 1;
tetsu_0207 0:eed6204ea3b1 120
tetsu_0207 0:eed6204ea3b1 121 area = 4;
tetsu_0207 0:eed6204ea3b1 122 }
tetsu_0207 0:eed6204ea3b1 123 if(area == 0){
tetsu_0207 0:eed6204ea3b1 124 rightPowerPercent = 0;
tetsu_0207 0:eed6204ea3b1 125 leftPowerPercent = 0;
tetsu_0207 0:eed6204ea3b1 126 }
tetsu_0207 0:eed6204ea3b1 127
tetsu_0207 0:eed6204ea3b1 128 // convert power persent to real power
tetsu_0207 0:eed6204ea3b1 129 powerL = allPower * leftPowerPercent;
tetsu_0207 0:eed6204ea3b1 130 powerR = allPower * rightPowerPercent;
tetsu_0207 0:eed6204ea3b1 131
tetsu_0207 0:eed6204ea3b1 132 // need to change power(power distance)
tetsu_0207 0:eed6204ea3b1 133 double needChangePowerL = powerL - currentPowerL;
tetsu_0207 0:eed6204ea3b1 134 double needChangePowerR = powerR - currentPowerR;
tetsu_0207 0:eed6204ea3b1 135 // change power
tetsu_0207 0:eed6204ea3b1 136 double changePowerL = needChangePowerL / POWER_CHANGE_PER_CLOCK;
tetsu_0207 0:eed6204ea3b1 137 double changePowerR = needChangePowerR / POWER_CHANGE_PER_CLOCK;
tetsu_0207 0:eed6204ea3b1 138 // set power
tetsu_0207 0:eed6204ea3b1 139 powerL = currentPowerL + changePowerL;
tetsu_0207 0:eed6204ea3b1 140 powerR = currentPowerR + changePowerR;
tetsu_0207 0:eed6204ea3b1 141 // save current power
tetsu_0207 0:eed6204ea3b1 142 currentPowerL = powerL;
tetsu_0207 0:eed6204ea3b1 143 currentPowerR = powerR;
tetsu_0207 0:eed6204ea3b1 144
tetsu_0207 0:eed6204ea3b1 145 //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 146
tetsu_0207 0:eed6204ea3b1 147 if(powerL >= 0){
tetsu_0207 0:eed6204ea3b1 148 dirL = DIR_DEFAULT_L;
tetsu_0207 0:eed6204ea3b1 149 }else{
tetsu_0207 0:eed6204ea3b1 150 powerL = -powerL;
tetsu_0207 0:eed6204ea3b1 151 dirL = !DIR_DEFAULT_L;
tetsu_0207 0:eed6204ea3b1 152 }
tetsu_0207 0:eed6204ea3b1 153 if(powerR >= 0){
tetsu_0207 0:eed6204ea3b1 154 dirR = DIR_DEFAULT_R;
tetsu_0207 0:eed6204ea3b1 155 }else{
tetsu_0207 0:eed6204ea3b1 156 powerR = -powerR;
tetsu_0207 0:eed6204ea3b1 157 dirR = !DIR_DEFAULT_R;
tetsu_0207 0:eed6204ea3b1 158 }
tetsu_0207 0:eed6204ea3b1 159
tetsu_0207 0:eed6204ea3b1 160 pwmL.write((float)powerL);
tetsu_0207 0:eed6204ea3b1 161 pwmR.write((float)powerR);
tetsu_0207 0:eed6204ea3b1 162
tetsu_0207 0:eed6204ea3b1 163 wait_ms(50);
tetsu_0207 0:eed6204ea3b1 164 //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 165 }
tetsu_0207 0:eed6204ea3b1 166 }
tetsu_0207 0:eed6204ea3b1 167
tetsu_0207 2:becb78cfc927 168 void control_rx(){
tetsu_0207 2:becb78cfc927 169 int head = 0;
tetsu_0207 2:becb78cfc927 170 char bytesX[4];
tetsu_0207 2:becb78cfc927 171 char bytesY[4];
tetsu_0207 2:becb78cfc927 172
tetsu_0207 2:becb78cfc927 173 while(serial.readable()){
tetsu_0207 2:becb78cfc927 174 char c = serial.getc();
tetsu_0207 2:becb78cfc927 175
tetsu_0207 2:becb78cfc927 176 if(c == 0x3a){
tetsu_0207 2:becb78cfc927 177 head = 0;
tetsu_0207 2:becb78cfc927 178 continue;
tetsu_0207 2:becb78cfc927 179 }
tetsu_0207 2:becb78cfc927 180 if(head < 3){
tetsu_0207 2:becb78cfc927 181 bytesX[head] = c;
tetsu_0207 2:becb78cfc927 182 }else{
tetsu_0207 2:becb78cfc927 183 bytesY[head - 3] = c;
tetsu_0207 2:becb78cfc927 184 }
tetsu_0207 2:becb78cfc927 185 if(head == 6){
tetsu_0207 2:becb78cfc927 186 // printf
tetsu_0207 2:becb78cfc927 187 int32_t intX = (bytesX[0] << 24) + (bytesX[1] << 16) + (bytesX[2] << 8) + bytesX[3];
tetsu_0207 2:becb78cfc927 188 int32_t intY = (bytesY[0] << 24) + (bytesY[1] << 16) + (bytesY[2] << 8) + bytesY[3];
tetsu_0207 2:becb78cfc927 189 // cast float to double
tetsu_0207 2:becb78cfc927 190 float formatX = (float) intX;
tetsu_0207 2:becb78cfc927 191 float formatY = (float) intY;
tetsu_0207 2:becb78cfc927 192 // save
tetsu_0207 2:becb78cfc927 193 x = formatX;
tetsu_0207 2:becb78cfc927 194 y = formatY;
tetsu_0207 2:becb78cfc927 195 }
tetsu_0207 2:becb78cfc927 196 head++;
tetsu_0207 2:becb78cfc927 197 }
tetsu_0207 2:becb78cfc927 198 }