Revision 67:6e183e1e9bcd, committed 2018-05-20

Comitter:

villetiukuvaara

Date:

Sun May 20 02:56:49 2018 +0000

Parent:

66:e408ae75c632

Commit message:

Testing out servo control.

Changed in this revision

diff -r e408ae75c632 -r 6e183e1e9bcd SerialTerm.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SerialTerm.lib	Sun May 20 02:56:49 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/Tobias/code/SerialTerm/#07da1704cfd8

diff -r e408ae75c632 -r 6e183e1e9bcd Servo.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo.lib	Sun May 20 02:56:49 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/villetiukuvaara/code/Servo/#9a48389dcf50

diff -r e408ae75c632 -r 6e183e1e9bcd WiiNunchuck.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/WiiNunchuck.lib	Sun May 20 02:56:49 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/knaka/code/WiiNunchuck/#9652195745e8

diff -r e408ae75c632 -r 6e183e1e9bcd main.cpp
--- a/main.cpp	Thu May 10 05:15:08 2018 +0100
+++ b/main.cpp	Sun May 20 02:56:49 2018 +0000
@@ -1,12 +1,151 @@
 #include "mbed.h"
+#include "WiiNunchuck.h"
+#include "Servo.h"
+
+#define LED1 PC_10
+#define LED2 PC_12
+#define LED3 PC_11
+
+#define SERVO1 PA_6
+#define SERVO2 PA_7
+#define SERVO3 PB_11
+#define SERVO4 PB_1
+#define SERVO5 PB_10
+#define SERVO6 PB_3
+
+//#define PWM_PERIOD 20 // ms
+//#define PULSE_MIN 500.0 // us
+//#define PULSE_MAX 2500.0 // us
+
+
+#define SWITCH1 PC_8
+#define SWITCH2 PC_6
+
+#define SDA PC_1
+#define SCL PC_0
+
+#define POW_EN PA_4
+
+WiiNunchuck nchk(SDA, SCL);
+Serial pc(USBTX, USBRX);
 
 DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalInOut pow_en(POW_EN);
+
+InterruptIn switch1(SWITCH1);
+InterruptIn switch2(SWITCH2);
+
+Servo servo_vert(SERVO1);
+Servo servo_horz(SERVO2);
+Servo servo_rot(SERVO4);
+Servo servo_grab(SERVO5);
+
+int accx, accy, accz;
+unsigned char joyx, joyy;
+bool buttonx, buttonz;
+
+void init();
+void switch1_irq();
+void switch2_irq();
+void enable_servos(bool on);
+void servo_angle(PwmOut *servo, float angle);
+
+bool servos_enabled = false;
+
 
 // main() runs in its own thread in the OS
-int main() {
-    while (true) {
-        led1 = !led1;
-        wait(0.5);
+int main()
+{
+    init();
+    
+    while(1)
+    {
+        char key = pc.getc();
+        if(key==0x1B)
+        {                               //If the keypress was an arrow key
+            key = pc.getc();                        //Check again!
+            if (key == 0x5B)
+            {
+                key = pc.getc();
+                switch(key) {                         //Check to see which arrow key...
+                    case 0x41:                        //It was the UP arrow key...
+                        servo_vert = servo_vert + 0.01;
+                        pc.printf("vert %f", servo_vert.read());
+                        break;
+                    case 0x42:                        //It was the DOWN arrow key...
+                        servo_vert = servo_vert - 0.01;
+                        pc.printf("vert %f", servo_vert.read());
+                        break;
+                    case 0x43:                        //It was the RIGHT arrow key...
+                        servo_rot = servo_rot + 0.01;
+                        pc.printf("\n\r RIGHT!");
+                        break;
+                    case 0x44:                        //It was the LEFT arrow key...
+                        servo_rot = servo_rot - 0.01;
+                        pc.printf("\n\r LEFT!");
+                        break;
+                }
+            }
+        }
+        
+        wait(0.01);  
     }
 }
 
+void init()
+{
+    pow_en.output();
+    pow_en.mode(OpenDrain);
+    enable_servos(false);
+    
+    servo_vert.calibrate(0.0007, 90)
+
+    servo_vert = 0.5;
+    servo_horz = 0.5;
+    servo_rot = 0.5;
+    servo_grab = 0.5;
+    /*servo_horz.position(90);
+    servo_rot.position(90);
+    servo_grab.position(90);*/
+    
+    switch1.mode(PullUp);
+    switch1.fall(&switch1_irq);
+    switch2.mode(PullUp);
+    switch2.fall(&switch2_irq);
+    
+    //led1 = 1;
+}
+
+void switch1_irq()
+{
+    if(servos_enabled)
+        enable_servos(false);
+    else
+        enable_servos(true);
+}
+
+void switch2_irq()
+{
+}
+
+void enable_servos(bool on)
+{
+    if(on)
+    {
+        pow_en.output();
+        pow_en = 0;
+        servos_enabled = true;
+        led2 = 1; 
+    }
+    else
+    {
+        pow_en = 1;
+        //pow_en.mode(OpenDrain);
+        pow_en.input();
+        servos_enabled = false;
+        led2 = 0; 
+    }
+}
+