Diff: main.cpp

Revision:

0:eed6204ea3b1

Child:

1:0ef4f75d84b5

diff -r 000000000000 -r eed6204ea3b1 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Oct 30 10:44:14 2020 +0000
@@ -0,0 +1,158 @@
+#include "mbed.h"
+
+//  Left Motor
+#define PWM_L A6
+#define DIR_L A3
+//  Right Motor
+#define PWM_R A2
+#define DIR_R D12
+//  default setting
+#define DIR_DEFAULT_L 1
+#define DIR_DEFAULT_R 0
+
+//  Math Function
+#define PI 3.14159265359
+//  power clock
+#define POWER_CHANGE_PER_CLOCK 5
+
+//  PWM
+PwmOut pwmL(PWM_L);
+//  DIR
+DigitalOut dirL(DIR_L);
+//  PWM
+PwmOut pwmR(PWM_R);
+//  DIR
+DigitalOut dirR(DIR_R);
+//  resistor
+//AnalogIn in(D3);
+//  joystick analogin
+AnalogIn joyX(A0);
+AnalogIn joyY(A1);
+
+//  current setting
+double currentPowerPerL = 0;
+double currentPowerPerR = 0; 
+//  current power
+double currentPowerL = 0;
+double currentPowerR = 0;
+
+int main()
+{
+    //printf("start program... \n\r");
+    //  period
+    pwmL.period_us(100);
+    pwmR.period_us(100);
+    //  Dir
+    dirL = DIR_DEFAULT_L;
+    dirR = DIR_DEFAULT_R;
+    //  power setting
+    float maxPower = 0.9F;
+    
+    //printf("finish start up! \n\r");
+
+    while(1){
+        //  prepare power value
+        float powerL,powerR;
+        
+        float x = joyX.read();  //  go ahead & back
+        float y = joyY.read();  //  left & right
+        //  value format
+        if(x > 1) x = 1.0;
+        if(x < 0) x = 0.0;
+        if(y > 1) y = 1.0;
+        if(y < 0) y = 0.0;
+        //  off set
+        //if(0.48F < x && x < 0.52F)x = 0.5F;
+        //if(0.48F < y && y < 0.52F)y = 0.5F;
+        
+        //  format x and y
+        double formatX = (2*x) - 1;
+        double formatY = (2*y) - 1;
+        //  tan
+        double tan = formatY / formatX;
+        //  arc tan
+        double arctan = (double) atan(tan);
+        //  angle
+        double angle = arctan * (180 / PI);
+        //  range
+        double range = sqrt((formatX * formatX) + (formatY * formatY));
+        if(range > 1.0F) range = 1.0F;
+        //  all power
+        double allPower = maxPower * (range / 1);
+        
+        //  right left power persent
+        double rightPowerPercent,leftPowerPercent;
+        int area = 0;
+        //1
+        if((0 < formatX && formatX < 1) && (0 < formatY && formatY < 1)){
+            rightPowerPercent = ((angle / 45) - 1) / 1;
+            leftPowerPercent = 1;
+            
+            area = 1;
+        }
+        //2
+        if((formatX < 0 && -1 < formatX) && (0 < formatY && formatY < 1)){
+            rightPowerPercent = 1;
+            leftPowerPercent = ((-angle / 45) - 1) / 1;
+            
+            area = 2;
+        }
+        //3
+        if((formatX < 0 && -1 < formatX) && (formatY < 0 && -1 < formatY)){
+            rightPowerPercent = ((-angle / 45) + 1) / 1;
+            leftPowerPercent = -1;
+        
+            area = 3;
+        }
+        //4
+        if((formatX > 0 && formatX < 1) && (formatY < 0 && formatY > -1)){
+            rightPowerPercent = -1;
+            leftPowerPercent = ((angle / 45) + 1) / 1;
+        
+            area = 4;
+        }
+        if(area == 0){
+            rightPowerPercent = 0;
+            leftPowerPercent = 0;
+        }
+        
+        //  convert power persent to real power
+        powerL = allPower * leftPowerPercent;
+        powerR = allPower * rightPowerPercent;
+        
+        //  need to change power(power distance)
+        double needChangePowerL = powerL - currentPowerL;
+        double needChangePowerR = powerR - currentPowerR;
+        //  change power
+        double changePowerL = needChangePowerL / POWER_CHANGE_PER_CLOCK;
+        double changePowerR = needChangePowerR / POWER_CHANGE_PER_CLOCK;
+        //  set power
+        powerL = currentPowerL + changePowerL;
+        powerR = currentPowerR + changePowerR;
+        //  save current power
+        currentPowerL = powerL;
+        currentPowerR = powerR;
+        
+        //printf("before:%lf %lf distance:%lf %lf change:%lf %lf after:%lf %lf \n\r",powerL,powerR,needChangePowerL,needChangePowerR,changePowerL,changePowerR,powerL,powerR);
+        
+        if(powerL >= 0){
+            dirL = DIR_DEFAULT_L;    
+        }else{
+            powerL = -powerL;
+            dirL = !DIR_DEFAULT_L;
+        }
+        if(powerR >= 0){
+            dirR = DIR_DEFAULT_R;    
+        }else{
+            powerR = -powerR;
+            dirR = !DIR_DEFAULT_R;
+        }
+        
+        pwmL.write((float)powerL);
+        pwmR.write((float)powerR);
+        
+        wait_ms(50);
+        //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);
+    }
+}
+