Diff: main.cpp

Revision:

0:dfd7b420339f

Child:

1:aec7284f51de

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Aug 26 08:22:16 2020 +0000
@@ -0,0 +1,119 @@
+/*
+ * ======================
+ * VS-C3/VS-RCV3 PIN
+ * ======================
+ * 1:NC  2:NC
+ * 3:DAT 4:CMD
+ * 5:SEL 6:CLK
+ * 7:+5~7V 8:NC
+ * 9:+3V 10:GND
+ *
+ */
+ 
+#include "mbed.h"
+#include "PS_PAD.h"
+
+class Motor{
+    PwmOut a, b;
+public:
+    Motor(PinName A_term, PinName B_term) : a(A_term), b(B_term) {
+        a.period(0.001);
+        b.period(0.001);
+    }
+    
+    Motor& operator=(double duty) {
+        if(duty > 0){
+            a = duty < 1 ? duty : 1;
+            b = 0;
+        }else {
+            a = 0;
+            b = duty > -1 ? -duty : -1;
+        }
+        return *this;
+    }
+};
+
+
+PS_PAD vsc3(PA_7, PA_6, PA_5, PA_4); //MOSI, MISO, SCK, SS
+
+DigitalIn lim[7] = {PD_2, PD_3, PD_4, PD_5, PD_6, PD_7, PG_9};
+Motor rmotor(PE_9, PE_11);
+Motor lmotor(PE_13, PE_14);
+Motor sxmotor(PA_0, PA_1);
+Motor tsmotor(PA_2, PA_3);
+Motor trmotor(PC_6, PC_7);
+Motor tlmotor(PC_8, PC_9);
+
+int main() {
+    vsc3.init();
+    double movey = 0, turn = 0;
+    printf("START\n");
+    
+    while(1) {
+        vsc3.poll();
+        if (vsc3.read(PS_PAD::PAD_TOP)){
+            movey = -1;
+        }else if(vsc3.read(PS_PAD::PAD_BOTTOM)){
+            movey = 1;
+        }else if(abs(vsc3.read(PS_PAD::ANALOG_LY)) > 10){
+            movey = (vsc3.read(PS_PAD::ANALOG_LY)) / 128.0;
+        }else{
+            movey = 0;
+        }
+               
+        if (vsc3.read(PS_PAD::PAD_RIGHT)){
+            turn = -1;
+        }else if(PS_PAD::PAD_LEFT){
+            turn = 1;
+        }else if(abs(vsc3.read(PS_PAD::ANALOG_LX)) > 10){
+            turn = (vsc3.read(PS_PAD::ANALOG_LX)) / 128.0;
+        }else{
+            turn = 0;
+        }
+        
+        rmotor = -movey+turn;      //move
+        lmotor = -movey-turn;
+        
+        if (vsc3.read(PS_PAD::PAD_CIRCLE) && lim[0] == 0){        //limitswitch
+            sxmotor = 1;
+        }else if(vsc3.read(PS_PAD::PAD_X) && lim[1] == 0){
+            sxmotor = -1;
+        }else if(vsc3.read(PS_PAD::PAD_CIRCLE) && vsc3.read(PS_PAD::PAD_X)){
+            sxmotor = 0;
+        }else{
+            sxmotor = 0;
+        }
+        
+        if (vsc3.read(PS_PAD::PAD_TRIANGLE) && lim[2] == 0){
+            tsmotor = 1;
+        }else if(vsc3.read(PS_PAD::PAD_SQUARE) && lim[3] == 0){
+            tsmotor = -1;
+        }else if(vsc3.read(PS_PAD::PAD_TRIANGLE) && vsc3.read(PS_PAD::PAD_SQUARE)){
+            tsmotor = 0;
+        }else{
+            tsmotor = 0;
+        }        
+        
+        if (vsc3.read(PS_PAD::PAD_R1) && lim[4] == 0){
+            trmotor = 1;
+        }else if(vsc3.read(PS_PAD::PAD_R2) && lim[5] == 0){
+            trmotor = -1;
+        }else if(vsc3.read(PS_PAD::PAD_R1) && vsc3.read(PS_PAD::PAD_R2)){
+            trmotor = 0;
+        }else{
+            trmotor = 0;
+        }
+                
+        if (vsc3.read(PS_PAD::PAD_L1) && lim[6] == 0){
+            tlmotor = 1;
+        }else if(vsc3.read(PS_PAD::PAD_L2) && lim[7] == 0){
+            tlmotor = -1;
+        }else if(vsc3.read(PS_PAD::PAD_L1) && vsc3.read(PS_PAD::PAD_L2)){
+            tlmotor = 0;
+        }else{
+            tlmotor = 0;
+        }
+        
+        wait(0.05);
+    }
+}
\ No newline at end of file