Ville Tiukuvaara / Mbed OS RobotArmControl

Controlling the servo expansion board at https://hackaday.io/project/157951-stm32-servo-expansion-board

Dependencies:   SerialTerm Servo WiiNunchuck

Fork of mbed-os-example-mbed5-blinky by mbed-os-examples

main.cpp@67:6e183e1e9bcd, 2018-05-20 (annotated)

Committer:
villetiukuvaara
Date:
Sun May 20 02:56:49 2018 +0000
Revision:
67:6e183e1e9bcd
Parent:
29:0b58d21e87d6 
Testing out servo control.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jonathan Austin 0:2757d7abb7d9 1 #include "mbed.h"
villetiukuvaara 67:6e183e1e9bcd 2 #include "WiiNunchuck.h"
villetiukuvaara 67:6e183e1e9bcd 3 #include "Servo.h"
villetiukuvaara 67:6e183e1e9bcd 4
villetiukuvaara 67:6e183e1e9bcd 5 #define LED1 PC_10
villetiukuvaara 67:6e183e1e9bcd 6 #define LED2 PC_12
villetiukuvaara 67:6e183e1e9bcd 7 #define LED3 PC_11
villetiukuvaara 67:6e183e1e9bcd 8
villetiukuvaara 67:6e183e1e9bcd 9 #define SERVO1 PA_6
villetiukuvaara 67:6e183e1e9bcd 10 #define SERVO2 PA_7
villetiukuvaara 67:6e183e1e9bcd 11 #define SERVO3 PB_11
villetiukuvaara 67:6e183e1e9bcd 12 #define SERVO4 PB_1
villetiukuvaara 67:6e183e1e9bcd 13 #define SERVO5 PB_10
villetiukuvaara 67:6e183e1e9bcd 14 #define SERVO6 PB_3
villetiukuvaara 67:6e183e1e9bcd 15
villetiukuvaara 67:6e183e1e9bcd 16 //#define PWM_PERIOD 20 // ms
villetiukuvaara 67:6e183e1e9bcd 17 //#define PULSE_MIN 500.0 // us
villetiukuvaara 67:6e183e1e9bcd 18 //#define PULSE_MAX 2500.0 // us
villetiukuvaara 67:6e183e1e9bcd 19
villetiukuvaara 67:6e183e1e9bcd 20
villetiukuvaara 67:6e183e1e9bcd 21 #define SWITCH1 PC_8
villetiukuvaara 67:6e183e1e9bcd 22 #define SWITCH2 PC_6
villetiukuvaara 67:6e183e1e9bcd 23
villetiukuvaara 67:6e183e1e9bcd 24 #define SDA PC_1
villetiukuvaara 67:6e183e1e9bcd 25 #define SCL PC_0
villetiukuvaara 67:6e183e1e9bcd 26
villetiukuvaara 67:6e183e1e9bcd 27 #define POW_EN PA_4
villetiukuvaara 67:6e183e1e9bcd 28
villetiukuvaara 67:6e183e1e9bcd 29 WiiNunchuck nchk(SDA, SCL);
villetiukuvaara 67:6e183e1e9bcd 30 Serial pc(USBTX, USBRX);
Jonathan Austin 0:2757d7abb7d9 31
Jonathan Austin 0:2757d7abb7d9 32 DigitalOut led1(LED1);
villetiukuvaara 67:6e183e1e9bcd 33 DigitalOut led2(LED2);
villetiukuvaara 67:6e183e1e9bcd 34 DigitalOut led3(LED3);
villetiukuvaara 67:6e183e1e9bcd 35 DigitalInOut pow_en(POW_EN);
villetiukuvaara 67:6e183e1e9bcd 36
villetiukuvaara 67:6e183e1e9bcd 37 InterruptIn switch1(SWITCH1);
villetiukuvaara 67:6e183e1e9bcd 38 InterruptIn switch2(SWITCH2);
villetiukuvaara 67:6e183e1e9bcd 39
villetiukuvaara 67:6e183e1e9bcd 40 Servo servo_vert(SERVO1);
villetiukuvaara 67:6e183e1e9bcd 41 Servo servo_horz(SERVO2);
villetiukuvaara 67:6e183e1e9bcd 42 Servo servo_rot(SERVO4);
villetiukuvaara 67:6e183e1e9bcd 43 Servo servo_grab(SERVO5);
villetiukuvaara 67:6e183e1e9bcd 44
villetiukuvaara 67:6e183e1e9bcd 45 int accx, accy, accz;
villetiukuvaara 67:6e183e1e9bcd 46 unsigned char joyx, joyy;
villetiukuvaara 67:6e183e1e9bcd 47 bool buttonx, buttonz;
villetiukuvaara 67:6e183e1e9bcd 48
villetiukuvaara 67:6e183e1e9bcd 49 void init();
villetiukuvaara 67:6e183e1e9bcd 50 void switch1_irq();
villetiukuvaara 67:6e183e1e9bcd 51 void switch2_irq();
villetiukuvaara 67:6e183e1e9bcd 52 void enable_servos(bool on);
villetiukuvaara 67:6e183e1e9bcd 53 void servo_angle(PwmOut *servo, float angle);
villetiukuvaara 67:6e183e1e9bcd 54
villetiukuvaara 67:6e183e1e9bcd 55 bool servos_enabled = false;
villetiukuvaara 67:6e183e1e9bcd 56
Jonathan Austin 0:2757d7abb7d9 57
Jonathan Austin 1:846c97078558 58 // main() runs in its own thread in the OS
villetiukuvaara 67:6e183e1e9bcd 59 int main()
villetiukuvaara 67:6e183e1e9bcd 60 {
villetiukuvaara 67:6e183e1e9bcd 61 init();
villetiukuvaara 67:6e183e1e9bcd 62
villetiukuvaara 67:6e183e1e9bcd 63 while(1)
villetiukuvaara 67:6e183e1e9bcd 64 {
villetiukuvaara 67:6e183e1e9bcd 65 char key = pc.getc();
villetiukuvaara 67:6e183e1e9bcd 66 if(key==0x1B)
villetiukuvaara 67:6e183e1e9bcd 67 { //If the keypress was an arrow key
villetiukuvaara 67:6e183e1e9bcd 68 key = pc.getc(); //Check again!
villetiukuvaara 67:6e183e1e9bcd 69 if (key == 0x5B)
villetiukuvaara 67:6e183e1e9bcd 70 {
villetiukuvaara 67:6e183e1e9bcd 71 key = pc.getc();
villetiukuvaara 67:6e183e1e9bcd 72 switch(key) { //Check to see which arrow key...
villetiukuvaara 67:6e183e1e9bcd 73 case 0x41: //It was the UP arrow key...
villetiukuvaara 67:6e183e1e9bcd 74 servo_vert = servo_vert + 0.01;
villetiukuvaara 67:6e183e1e9bcd 75 pc.printf("vert %f", servo_vert.read());
villetiukuvaara 67:6e183e1e9bcd 76 break;
villetiukuvaara 67:6e183e1e9bcd 77 case 0x42: //It was the DOWN arrow key...
villetiukuvaara 67:6e183e1e9bcd 78 servo_vert = servo_vert - 0.01;
villetiukuvaara 67:6e183e1e9bcd 79 pc.printf("vert %f", servo_vert.read());
villetiukuvaara 67:6e183e1e9bcd 80 break;
villetiukuvaara 67:6e183e1e9bcd 81 case 0x43: //It was the RIGHT arrow key...
villetiukuvaara 67:6e183e1e9bcd 82 servo_rot = servo_rot + 0.01;
villetiukuvaara 67:6e183e1e9bcd 83 pc.printf("\n\r RIGHT!");
villetiukuvaara 67:6e183e1e9bcd 84 break;
villetiukuvaara 67:6e183e1e9bcd 85 case 0x44: //It was the LEFT arrow key...
villetiukuvaara 67:6e183e1e9bcd 86 servo_rot = servo_rot - 0.01;
villetiukuvaara 67:6e183e1e9bcd 87 pc.printf("\n\r LEFT!");
villetiukuvaara 67:6e183e1e9bcd 88 break;
villetiukuvaara 67:6e183e1e9bcd 89 }
villetiukuvaara 67:6e183e1e9bcd 90 }
villetiukuvaara 67:6e183e1e9bcd 91 }
villetiukuvaara 67:6e183e1e9bcd 92
villetiukuvaara 67:6e183e1e9bcd 93 wait(0.01);
Jonathan Austin 0:2757d7abb7d9 94 }
Jonathan Austin 0:2757d7abb7d9 95 }
Jonathan Austin 1:846c97078558 96
villetiukuvaara 67:6e183e1e9bcd 97 void init()
villetiukuvaara 67:6e183e1e9bcd 98 {
villetiukuvaara 67:6e183e1e9bcd 99 pow_en.output();
villetiukuvaara 67:6e183e1e9bcd 100 pow_en.mode(OpenDrain);
villetiukuvaara 67:6e183e1e9bcd 101 enable_servos(false);
villetiukuvaara 67:6e183e1e9bcd 102
villetiukuvaara 67:6e183e1e9bcd 103 servo_vert.calibrate(0.0007, 90)
villetiukuvaara 67:6e183e1e9bcd 104
villetiukuvaara 67:6e183e1e9bcd 105 servo_vert = 0.5;
villetiukuvaara 67:6e183e1e9bcd 106 servo_horz = 0.5;
villetiukuvaara 67:6e183e1e9bcd 107 servo_rot = 0.5;
villetiukuvaara 67:6e183e1e9bcd 108 servo_grab = 0.5;
villetiukuvaara 67:6e183e1e9bcd 109 /*servo_horz.position(90);
villetiukuvaara 67:6e183e1e9bcd 110 servo_rot.position(90);
villetiukuvaara 67:6e183e1e9bcd 111 servo_grab.position(90);*/
villetiukuvaara 67:6e183e1e9bcd 112
villetiukuvaara 67:6e183e1e9bcd 113 switch1.mode(PullUp);
villetiukuvaara 67:6e183e1e9bcd 114 switch1.fall(&switch1_irq);
villetiukuvaara 67:6e183e1e9bcd 115 switch2.mode(PullUp);
villetiukuvaara 67:6e183e1e9bcd 116 switch2.fall(&switch2_irq);
villetiukuvaara 67:6e183e1e9bcd 117
villetiukuvaara 67:6e183e1e9bcd 118 //led1 = 1;
villetiukuvaara 67:6e183e1e9bcd 119 }
villetiukuvaara 67:6e183e1e9bcd 120
villetiukuvaara 67:6e183e1e9bcd 121 void switch1_irq()
villetiukuvaara 67:6e183e1e9bcd 122 {
villetiukuvaara 67:6e183e1e9bcd 123 if(servos_enabled)
villetiukuvaara 67:6e183e1e9bcd 124 enable_servos(false);
villetiukuvaara 67:6e183e1e9bcd 125 else
villetiukuvaara 67:6e183e1e9bcd 126 enable_servos(true);
villetiukuvaara 67:6e183e1e9bcd 127 }
villetiukuvaara 67:6e183e1e9bcd 128
villetiukuvaara 67:6e183e1e9bcd 129 void switch2_irq()
villetiukuvaara 67:6e183e1e9bcd 130 {
villetiukuvaara 67:6e183e1e9bcd 131 }
villetiukuvaara 67:6e183e1e9bcd 132
villetiukuvaara 67:6e183e1e9bcd 133 void enable_servos(bool on)
villetiukuvaara 67:6e183e1e9bcd 134 {
villetiukuvaara 67:6e183e1e9bcd 135 if(on)
villetiukuvaara 67:6e183e1e9bcd 136 {
villetiukuvaara 67:6e183e1e9bcd 137 pow_en.output();
villetiukuvaara 67:6e183e1e9bcd 138 pow_en = 0;
villetiukuvaara 67:6e183e1e9bcd 139 servos_enabled = true;
villetiukuvaara 67:6e183e1e9bcd 140 led2 = 1;
villetiukuvaara 67:6e183e1e9bcd 141 }
villetiukuvaara 67:6e183e1e9bcd 142 else
villetiukuvaara 67:6e183e1e9bcd 143 {
villetiukuvaara 67:6e183e1e9bcd 144 pow_en = 1;
villetiukuvaara 67:6e183e1e9bcd 145 //pow_en.mode(OpenDrain);
villetiukuvaara 67:6e183e1e9bcd 146 pow_en.input();
villetiukuvaara 67:6e183e1e9bcd 147 servos_enabled = false;
villetiukuvaara 67:6e183e1e9bcd 148 led2 = 0;
villetiukuvaara 67:6e183e1e9bcd 149 }
villetiukuvaara 67:6e183e1e9bcd 150 }
villetiukuvaara 67:6e183e1e9bcd 151