Diff: syudouki.cpp

Revision:

1:797913662b1f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/syudouki.cpp	Sun Jul 15 09:12:49 2018 +0000
@@ -0,0 +1,126 @@
+#include "mbed.h"
+#include "kbt.h"
+
+KBT kbt(PA_9,PA_10);
+Serial pc(USBTX,USBRX);
+
+PwmOut rifrpl (PC_12);
+PwmOut rifrmi (PA_13);
+PwmOut ribapl (PA_14);
+PwmOut ribami (PA_15);
+PwmOut lefrpl (PB_7);
+PwmOut lefrmi (PC_13);
+PwmOut lebapl (PF_0);
+PwmOut lebami (PC_2);
+PwmOut servo (PC_3);
+DigitalOut air (PB_10);
+
+double map(double x, double in_min, double in_max, double out_min, double out_max)
+{
+  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+
+double Abs ( double value ){
+    if ( value < 0 )
+      value = value*(-1);
+    return value;
+    }
+
+int main() {
+    
+    kbt.init(2400);
+    pc.baud(9600);
+    
+    while(1) {
+        double Larpow = kbt.Stick[L_around];
+        double Luppow = kbt.Stick[L_updown];
+        
+        int dir;
+        
+        double pow = 0;
+        
+        double Larpowabs = Abs(Larpow);
+        double Luppowabs = Abs(Luppow);
+        
+        if (Larpowabs > Luppowabs){
+           if (Larpow < 0){
+              dir = 0;
+              pow = Larpow*(-1);
+              }
+           else if (Larpow > 0){
+              dir = 1;
+              pow = Larpow;
+              }
+        }      
+        else {
+            if (Luppow < 0){
+                dir = 2;
+                pow = Luppow*(-1);
+                } 
+            else if (Luppow > 0){
+                dir = 3;
+                pow = Luppow;
+                }
+        }        
+        
+        pow = map(pow,0.0,255.0,0.0,1.0);
+        
+        switch (dir){
+            case 0 :
+               rifrpl = 0;
+               rifrmi = pow;
+               ribapl = pow;
+               ribami = 0;
+               lefrpl = pow;
+               lefrmi = 0;
+               lebapl = 0;
+               lebami = pow;
+               break;
+            case 1 :
+               rifrpl = pow;
+               rifrmi = 0;
+               ribapl = 0;
+               ribami = pow;
+               lefrpl = 0;
+               lefrmi = pow;
+               lebapl = pow;
+               lebami = 0;
+               break;
+            case 2 :
+               rifrpl = 0;
+               rifrmi = pow;
+               ribapl = 0;
+               ribami = pow;
+               lefrpl = 0;
+               lefrmi = pow;
+               lebapl = 0;
+               lebami = pow;
+               break;
+            case 3 :
+               rifrpl = pow;
+               rifrmi = 0;
+               ribapl = pow;
+               ribami = 0;
+               lefrpl = pow;
+               lefrmi = 0;
+               lebapl = pow;
+               lebami = 0;
+               break;
+        }
+        
+        bool ser = kbt.Button[circle];
+        servo.period_ms(20);
+        if (ser == 1){
+           servo.pulsewidth_us(700);
+           wait(8);
+           servo.pulsewidth_us(1500);
+           wait(8);
+           }
+        
+        bool ai = kbt.Button[triangle];
+        if (ai == 1)
+           air = 1;
+        else
+           air = 0;
+     }
+} 
\ No newline at end of file