Diff: main.cpp

Revision:

0:158455b60200

Child:

1:681efc14eba9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Jan 19 12:13:21 2019 +0000
@@ -0,0 +1,163 @@
+#include "mbed.h"
+#include "speedservo.h"
+
+//肘
+Servo RFelbow(PA_13);
+Servo LFelbow(PA_15);
+Servo RBelbow(PC_2);
+Servo LBelbow(PC_13);
+
+//膝
+Servo RFknee(PA_14);
+Servo LFknee(PC_1);
+Servo RBknee(PC_3);
+Servo LBknee(PC_7);
+
+//肩
+Serial SerialServo(PC_4,PA_10);
+
+DigitalIn Dmode(PB_13);//歩行方向変更
+DigitalIn start_button(PB_2);
+
+AnalogIn var(PC_0);
+Serial PC(USBTX,USBRX);
+
+int ser_spe[4] = {32,32,32,32};
+int ser_pos[4] = {135,135,135,135};
+int now_pos[4] = {};
+
+int prePos = 2000;
+int behPos = 1700;
+int upPos = 850;
+int downPos = 1350;
+
+float fmap(float x, float in_min, float in_max, float out_min, float out_max){
+  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+
+
+void serServoMove(int num,int angle,int speed){
+    ser_spe[num] = speed;
+    ser_pos[num] = angle;
+}
+
+void bluepill_rx(){
+    if(SerialServo.readable()){
+        int request = SerialServo.getc();
+        if(request >= 0 && request <= 3){
+            now_pos[request] = SerialServo.getc();
+            SerialServo.putc(request);
+            SerialServo.putc(ser_spe[request]);
+            SerialServo.putc(ser_pos[request]);
+        }
+    }
+}
+void centerMove(int dev,int num){
+    for(int i=0;i<4;i++){
+        serServoMove(i,135+dev,30);
+    }
+        /*
+        if(i != num){
+            serServoMove(i,135+dev,30);
+        }
+    }
+    */
+    
+}
+
+void RFmove(int position,int speed){
+    if(2350 <= position){
+        position = 2350;
+    }
+    RFelbow.SpeedConvert(speed);
+    RFelbow.SetPosition(fmap(position,850,2350,2350,850));
+}
+
+void LFmove(int position,int speed){
+    if(2350 <= position){
+        position = 2350;
+    }
+    
+    LFelbow.SpeedConvert(speed);
+    LFelbow.SetPosition(position);
+}
+
+void RBmove(int position,int speed){
+    if(2350 <= position){
+        position = 2350;
+    }
+    
+    RBelbow.SpeedConvert(speed);
+    RBelbow.SetPosition(position);
+}
+
+void LBmove(int position,int speed){
+    if(2350 <= position){
+        position = 2350;
+    }
+    
+    LBelbow.SpeedConvert(speed);
+    LBelbow.SetPosition(fmap(position,850,2350,2350,850));
+}
+
+void forwarding(){
+    RFmove(1600,3);
+    LFmove(1600,3);
+    RBmove(2200,3);
+    LBmove(2200,3);
+}
+
+float angle = 0;
+int main() {
+    //初期設定
+    
+    SerialServo.baud(115200);
+    SerialServo.attach(bluepill_rx, Serial::RxIrq);
+    RFelbow.Enable(fmap(behPos,850,2350,2350,850),330,10);
+    LFelbow.Enable(behPos,330,10);
+    RBelbow.Enable(prePos,330,10);
+    LBelbow.Enable(fmap(prePos,850,2350,2350,850),330,10);
+
+    RFknee.Enable(downPos,330,50);
+    LFknee.Enable(downPos,330,50);
+    RBknee.Enable(downPos-100,330,50);
+    LBknee.Enable(downPos-100,330,50);
+
+    serServoMove(0,135,100);
+    serServoMove(1,135,100);
+    serServoMove(2,135,100);
+    serServoMove(3,135,100);
+    forwarding();
+    wait(5);
+    while (true) {
+        angle = int(20*var);
+        PC.printf("%f \n",angle);        
+        centerMove(angle,0);
+        RBknee.SetPosition(downPos -(var*300));
+        LBknee.SetPosition(downPos -(var*300));
+        
+        if(Dmode == true){
+            //RFmove(prePos,50);
+            RFknee.SetPosition(upPos);
+            wait(2.0);
+            RFmove(2350,10);
+            wait(2.0);
+            RFknee.SetPosition(downPos);
+            wait(2.0);
+            RFmove(prePos,25);
+            wait(2.0);
+            centerMove(0,0);
+            RBknee.SetPosition(downPos);
+            LBknee.SetPosition(downPos);
+            
+            RBknee.SetPosition(downPos);
+            LBknee.SetPosition(downPos);
+            
+        }else{
+            RFknee.SetPosition(downPos);
+            forwarding();
+        }
+        
+        wait(0.1);
+   }
+}
\ No newline at end of file