Revision 0:1fdc57edebb6, committed 2021-06-30

Comitter:

andre44s

Date:

Wed Jun 30 04:08:30 2021 +0000

Commit message:

Working code for STM32F407VET6 Master

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RF24.lib	Wed Jun 30 04:08:30 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/hudakz/code/RF24/#d96c2056bf37

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Jun 30 04:08:30 2021 +0000
@@ -0,0 +1,508 @@
+#include "mbed.h"
+#include "RF24.h"
+#include <cstdlib>
+
+//HIGH LOW Function
+int low = 0;
+int high = 1;
+
+//MAP Function
+long map(long x, long in_min, long in_max, long out_min, long out_max)
+{
+  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+
+//NRF24 Setting
+const uint64_t  address = 0xF0F0F0F0F0F0F001L;
+RF24            radio(PB_5, PB_4, PB_3, PB_7, PB_6);    // mosi, miso, sck, csn, ce, (irq not used)
+
+struct Data_Package {
+    uint8_t LX;
+    uint8_t LY;
+    uint8_t RX;
+    uint8_t RY;
+    uint8_t XButton;
+    uint8_t TButton;
+    uint8_t SButton;
+    uint8_t CButton;
+    uint8_t UButton;
+    uint8_t DButton;
+    uint8_t LButton;
+    uint8_t RButton;
+    uint8_t L1;
+    uint8_t L2;
+    uint8_t L3;
+    uint8_t R1;
+    uint8_t R2;
+    uint8_t R3;
+};
+Data_Package data;
+
+//Motor Variable
+int rpm_y = 0, rpm_x = 0;
+float rpm1, rpm2, rpm3, rpm4, rotateSpeed;
+int setRPM1 = 0, setRPM2 = 0, setRPM3 = 0, setRPM4 = 0;
+float max_rpm = 200 , pwm_freq = 0.0001;
+int currentRPM1,currentRPM2,currentRPM3,currentRPM4,currentRPM5,currentRPM6;
+int encCount1 = 540;
+int encCount2 = 540;
+int encCount3 = 540;
+int encCount4 = 540;
+
+float dataGyro = 0, yawTarget = 0;
+
+//PID_1
+float PIDValue1 = 0, gainValue1 = 0, pwmValue1 = 0;
+float error1, prevError1;
+float P_1 = 0, I_1 = 0, D_1 = 0;
+float Kp1 = 0.0005, Ki1 = 0, Kd1 = 0; //Atur
+
+//PID_2
+float PIDValue2 = 0, gainValue2 = 0, pwmValue2 = 0;
+float error2, prevError2;
+float P_2 = 0, I_2 = 0, D_2 = 0;
+float Kp2 = 0.0005, Ki2 = 0, Kd2 = 0; //Atur
+
+//PID_3
+float PIDValue3 = 0, gainValue3 = 0, pwmValue3 = 0;
+float error3, prevError3;
+float P_3 = 0, I_3 = 0, D_3 = 0;
+float Kp3 = 0.0005, Ki3 = 0, Kd3 = 0; //Atur
+
+//PID_4
+float PIDValue4 = 0, gainValue4 = 0, pwmValue4 = 0;
+float error4, prevError4;
+float P_4 = 0, I_4 = 0, D_4 = 0;
+float Kp4 = 0.0005, Ki4 = 0, Kd4 = 0; //Atur
+
+//Serial Communication
+Serial serial1(PA_9,PA_10); //TX,RX
+
+//Interrupt Declaration
+InterruptIn enc1a(PE_8);
+InterruptIn enc1b(PE_9);
+InterruptIn enc2a(PE_10);
+InterruptIn enc2b(PE_11);
+InterruptIn enc3a(PA_0);
+InterruptIn enc3b(PA_1);
+InterruptIn enc4a(PA_2);
+InterruptIn enc4b(PA_3);
+InterruptIn enc5a(PD_12);
+InterruptIn enc5b(PD_13);
+InterruptIn enc6a(PD_14);
+InterruptIn enc6b(PD_15);
+
+//Motor PIN
+PwmOut pwm1(PB_0);
+PwmOut pwm2(PB_1);
+PwmOut pwm3(PB_8);
+PwmOut pwm4(PB_9);
+
+DigitalOut ena1a(PC_4);
+DigitalOut ena1b(PA_4);
+DigitalOut ena2a(PA_5);
+DigitalOut ena2b(PC_5);
+DigitalOut ena3a(PD_7); //ext11 rubah
+DigitalOut ena3b(PE_1);
+DigitalOut ena4a(PE_0);
+DigitalOut ena4b(PD_3); //ext10 rubah
+
+AnalogIn mcs1(PC_0);
+AnalogIn mcs2(PC_1);
+AnalogIn mcs3(PC_2);
+AnalogIn mcs4(PC_3);
+
+Timer t;
+
+//Encoder PIN & Variable
+int rev1a = 0;
+int rev2a = 0;
+int rev3a = 0;
+int rev4a = 0;
+int rev5a = 0;
+int rev6a = 0;
+int rev1b = 0;
+int rev2b = 0;
+int rev3b = 0;
+int rev4b = 0;
+int rev5b = 0;
+int rev6b = 0;
+
+//100ms Timer
+float currentMillis = 0, prevMillis = 0;
+
+void incr1a(){
+    rev1a=rev1a+1;
+}
+void incr1b(){
+    rev1b=rev1b+1;
+}
+
+void incr2a(){
+    rev2a=rev2a+1;
+}
+void incr2b(){
+    rev2b=rev2b+1;
+}
+
+void incr3a(){
+    rev3a=rev3a+1;
+}
+void incr3b(){
+    rev3b=rev3b+1;
+}
+
+void incr4a(){
+    rev4a=rev4a+1;
+}
+void incr4b(){
+    rev4b=rev4b+1;
+}
+
+void incr5a(){
+    rev5a=rev5a+1;
+}
+void incr5b(){
+    rev5b=rev5b+1;
+}
+
+void incr6a(){
+    rev6a=rev6a+1;
+}
+void incr6b(){
+    rev6b=rev6b+1;
+}
+
+void bacaRemote() {
+  //Cek adakah input dari controller
+    if (data.LX != 128 || data.LY != 128 || data.RX != 128) {
+        //Move
+        if ((data.LY != 128 || data.LX != 128) && data.RX == 128) {
+            //serial1.printf("Move : %d  %d\n", data.LX, data.LY);
+            setRPM1 = rpm_y - rpm_x;
+            setRPM2 = rpm_y + rpm_x;
+            setRPM3 = rpm_y + rpm_x;
+            setRPM4 = rpm_y - rpm_x;
+        }
+
+        //Rotate
+        else if (data.LY == 128 && data.LX == 128 && data.RX != 128) {
+            //serial1.printf("Putar Kiri : %f\n", rotateSpeed);
+            setRPM1 = -rotateSpeed;
+            setRPM2 = -rotateSpeed;
+            setRPM3 = rotateSpeed;
+            setRPM4 = rotateSpeed;
+            yawTarget = dataGyro;
+        }
+    }
+
+    //Stop Motor
+    if (data.LY == 128 && data.LX == 128 && data.RX == 128) {
+        setRPM1 = 0;
+        setRPM2 = 0;
+        setRPM3 = 0;
+        setRPM4 = 0;
+        //serial1.printf("Stop\n");
+    }
+}
+
+//PID MOTOR 1
+void PID1(){
+    error1 = abs(setRPM1)-currentRPM1;
+    P_1 = error1;
+    I_1 = I_1 + error1;
+    D_1 = error1 - prevError1;
+    
+    if (I_1 > 1){
+        I_1 = 1;
+    }
+        
+    if (I_1 < 0){
+        I_1 = 0;
+    }
+    
+    PIDValue1 = (Kp1 * P_1) + (Ki1 * P_1);
+    prevError1 = error1;
+    pwmValue1 = pwmValue1 + PIDValue1;
+    
+    if(pwmValue1 >= 1){
+        pwmValue1 = 1;
+    }
+    
+    if(pwmValue1 < 0){
+        pwmValue1 = 0;
+    }
+    
+    if(setRPM1 == 0){
+        pwmValue1 = 0;
+        pwm1.period(pwm_freq);
+        pwm1.write(0.5);
+        ena1a=low;
+        ena1b=low;
+    }   
+    
+    else if(setRPM1 > 0){
+        pwm1.period(pwm_freq);
+        pwm1.write(pwmValue1);
+        ena1a=high;
+        ena1b=low;
+    }
+    
+    else{
+        pwm1.period(pwm_freq);
+        pwm1.write(pwmValue1);
+        ena1a=low;
+        ena1b=high;    
+    }
+}
+
+//PID MOTOR 2
+void PID2(){
+    error2 = abs(setRPM2)-currentRPM2;
+    P_2 = error2;
+    I_2 = I_2 + error2;
+    D_2 = error2 - prevError2;
+    
+    if (I_2 > 100){
+        I_2 = 100;
+    }
+        
+    if (I_2 < 0){
+        I_2 = 0;
+    }
+    
+    PIDValue2 = (Kp2 * P_2) + (Ki2 * P_2);
+    prevError2 = error2;
+    pwmValue2 = pwmValue2 + PIDValue2;
+    
+    if(pwmValue2 >= 100){
+        pwmValue2 = 100;
+    }
+    
+    if(pwmValue2 < 0){
+        pwmValue2 = 0;
+    }
+    
+    if(setRPM2 == 0){
+        pwmValue2 = 0;
+        pwm2.period(pwm_freq);
+        pwm2.write(0.8);
+        ena2a=low;
+        ena2b=low;
+    }   
+    
+    else if(setRPM2 > 0){
+        pwm2.period(pwm_freq);
+        pwm2.write(pwmValue1);
+        ena2a=high;
+        ena2b=low;
+    }
+    
+    else{
+        pwm2.period(pwm_freq);
+        pwm2.write(pwmValue1);
+        ena2a=low;
+        ena2b=high;    
+    }
+}
+
+//PID MOTOR 3
+void PID3(){
+    error3 = abs(setRPM3)-currentRPM3;
+    P_3 = error3;
+    I_3 = I_3 + error3;
+    D_3 = error3 - prevError3;
+    
+    if (I_3 > 100){
+        I_3 = 100;
+    }
+        
+    if (I_3 < 0){
+        I_3 = 0;
+    }
+    
+    PIDValue3 = (Kp3 * P_3) + (Ki3 * P_3);
+    prevError3 = error3;
+    pwmValue3 = pwmValue3 + PIDValue3;
+    
+    if(pwmValue3 >= 100){
+        pwmValue3 = 100;
+    }
+    
+    if(pwmValue3 < 0){
+        pwmValue3 = 0;
+    }
+    
+    if(setRPM3 == 0){
+        pwmValue3 = 0;
+        pwm3.period(pwm_freq);
+        pwm3.write(0.1);
+        ena3a=low;
+        ena3b=low;
+    }   
+    
+    else if(setRPM3 > 0){
+        pwm3.period(pwm_freq);
+        pwm3.write(pwmValue3);
+        ena3a=high;
+        ena3b=low;
+    }
+    
+    else{
+        pwm3.period(pwm_freq);
+        pwm3.write(pwmValue3);
+        ena3a=low;
+        ena3b=high;    
+    }
+}
+
+//PID MOTOR 4
+void PID4(){
+    error4 = abs(setRPM4)-currentRPM4;
+    P_4 = error4;
+    I_4 = I_4 + error4;
+    D_4 = error4 - prevError4;
+    
+    if (I_4 > 100){
+        I_4 = 100;
+    }
+        
+    if (I_4 < 0){
+        I_4 = 0;
+    }
+    
+    PIDValue4 = (Kp4 * P_4) + (Ki4 * P_4);
+    prevError4 = error4;
+    pwmValue4 = pwmValue4 + PIDValue4;
+    
+    if(pwmValue4 >= 100){
+        pwmValue4 = 100;
+    }
+    
+    if(pwmValue4 < 0){
+        pwmValue4 = 0;
+    }
+    
+    if(setRPM4 == 0){
+        pwm4.period(pwm_freq);
+        pwmValue4 = 0;
+        pwm4.write(0.3);
+        ena4a=low;
+        ena4b=low;
+    }   
+    
+    else if(setRPM4 > 0){
+        pwm4.period(pwm_freq);
+        pwm4.write(pwmValue4);
+        ena4a=high;
+        ena4b=low;
+    }
+    
+    else{
+        pwm4.period(pwm_freq);
+        pwm4.write(pwmValue4);
+        ena4a=low;
+        ena4b=high;    
+    }
+}
+
+
+int main(void)
+{
+    serial1.baud(2000000);
+    radio.begin();
+    radio.openReadingPipe(0, address);
+    radio.setAutoAck(false);
+    radio.setDataRate(RF24_250KBPS);
+    radio.setPALevel(RF24_PA_MIN);
+    radio.startListening();
+    
+    data.LX = 128;
+    data.LY = 128;
+    data.RX = 128;
+    data.RY = 128;
+    data.XButton = 0;
+    data.TButton = 0;
+    data.SButton = 0;
+    data.CButton = 0;
+    data.UButton = 0;
+    data.DButton = 0;
+    data.LButton = 0;
+    data.RButton = 0;
+    data.L1 = 0;
+    data.L2 = 0;
+    data.L3 = 0;
+    data.R1 = 0;
+    data.R2 = 0;
+    data.R3 = 0;
+    
+    //Rising Edge
+    enc1a.rise(&incr1a);
+    enc1b.rise(&incr1b);
+    enc2a.rise(&incr2a);
+    enc2b.rise(&incr2b);
+    enc3a.rise(&incr3a);
+    enc3b.rise(&incr3b);
+    enc4a.rise(&incr4a);
+    enc4b.rise(&incr4b);
+    enc5a.rise(&incr5a);
+    enc5b.rise(&incr5b);
+    enc6a.rise(&incr6a);
+    enc6b.rise(&incr6b);
+    //Falling Edge
+    enc1a.fall(&incr1a);
+    enc1b.fall(&incr1b);
+    enc2a.fall(&incr2a);
+    enc2b.fall(&incr2b);
+    enc3a.fall(&incr3a);
+    enc3b.fall(&incr3b);
+    enc4a.fall(&incr4a);
+    enc4b.fall(&incr4b);
+    enc5a.fall(&incr5a);
+    enc5b.fall(&incr5b);
+    enc6a.fall(&incr6a);
+    enc6b.fall(&incr6b);
+    
+    t.start();
+    prevMillis = 0 + t.read();
+    
+    while (1) {
+        if (radio.available()) {
+            radio.read(&data, sizeof(Data_Package)); // read message and send acknowledge back to the master
+        }
+        
+        currentMillis = 0 + t.read();
+        if(currentMillis - prevMillis >= 0.099992){
+            prevMillis = currentMillis;
+            
+            currentRPM1 = (rev1a + rev1b) * 600 / encCount1;
+            currentRPM2 = (rev2a + rev2b) * 600 / encCount2;
+            currentRPM3 = (rev3a + rev3b) * 600 / encCount3;
+            currentRPM4 = (rev4a + rev4b) * 600 / encCount4;
+            currentRPM5 = (rev5a + rev5b) * 600 / 540;
+            currentRPM6 = (rev6a + rev6b) * 600 / 540;
+            
+            
+            PID1();
+            PID2();
+            PID3();
+            PID4();
+            
+            rev1a=0;
+            rev2a=0;
+            rev3a=0;
+            rev4a=0;
+            rev1b=0;
+            rev2b=0;
+            rev3b=0;
+            rev4b=0;
+            
+            serial1.printf("%d  %d  %d  %d\n", currentRPM1, currentRPM2, currentRPM3, currentRPM4);
+        }
+
+        rpm_x = (map(data.LX, 0, 255, -100, 100) * max_rpm) / 100;
+        rpm_y = (map(data.LY, 0, 255, -100, 100) * max_rpm) / 100;
+        rotateSpeed = (map(data.RX, 0, 255, -100, 100) * max_rpm) / 200;
+        bacaRemote();
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Jun 30 04:08:30 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file