Myself / Mbed 2 deprecated testing_servo_control

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@0:022cc499efd8, 2020-07-05 (annotated)

Committer:
wcyam2
Date:
Sun Jul 05 10:20:31 2020 +0000
Revision:
0:022cc499efd8
testing

Who changed what in which revision?

UserRevisionLine numberNew contents of line
wcyam2 0:022cc499efd8 1 /*
wcyam2 0:022cc499efd8 2 ######## Project name: servo_control
wcyam2 0:022cc499efd8 3 #readme# Program creater: Chong
wcyam2 0:022cc499efd8 4 ######## Last edit: 19/6/2020
wcyam2 0:022cc499efd8 5
wcyam2 0:022cc499efd8 6 [aims]
wcyam2 0:022cc499efd8 7 This program is to test the drive the servo from middle position.
wcyam2 0:022cc499efd8 8
wcyam2 0:022cc499efd8 9 [board]
wcyam2 0:022cc499efd8 10 NUCLEO-F401RE
wcyam2 0:022cc499efd8 11 F446RE_Universal_board with button
wcyam2 0:022cc499efd8 12 Creater: Big Vic
wcyam2 0:022cc499efd8 13 Date: 29/4/2020
wcyam2 0:022cc499efd8 14 [input] button design
wcyam2 0:022cc499efd8 15 A5 A4 A3 A2 A1 A0
wcyam2 0:022cc499efd8 16 start read +1 reset -1 read
wcyam2 0:022cc499efd8 17
wcyam2 0:022cc499efd8 18 #after start
wcyam2 0:022cc499efd8 19 A5 A4 A3 A2 A1 A0
wcyam2 0:022cc499efd8 20 prev max +% mid -% min
wcyam2 0:022cc499efd8 21
wcyam2 0:022cc499efd8 22 [output]
wcyam2 0:022cc499efd8 23 D3
wcyam2 0:022cc499efd8 24 pwm
wcyam2 0:022cc499efd8 25
wcyam2 0:022cc499efd8 26 [flow]
wcyam2 0:022cc499efd8 27 initialize:
wcyam2 0:022cc499efd8 28 blink onboard led 2 second
wcyam2 0:022cc499efd8 29 servo position = middle position
wcyam2 0:022cc499efd8 30
wcyam2 0:022cc499efd8 31 main:
wcyam2 0:022cc499efd8 32 change postition depend on the button pressed
wcyam2 0:022cc499efd8 33 # only button [+5%(A3)] and [-5%(A1)] will change the position after release the button
wcyam2 0:022cc499efd8 34 # other button will change the position immediately
wcyam2 0:022cc499efd8 35
wcyam2 0:022cc499efd8 36 [user guide]
wcyam2 0:022cc499efd8 37 goto user setting
wcyam2 0:022cc499efd8 38 check the period / hertz of the servo e.g. 0.020f
wcyam2 0:022cc499efd8 39 check the minimum and maximum valid pulsewidth e.g. 500, 2500
wcyam2 0:022cc499efd8 40 check the minimum and maximum valid degree [0,360] e.g. 0, 180
wcyam2 0:022cc499efd8 41
wcyam2 0:022cc499efd8 42
wcyam2 0:022cc499efd8 43 */
wcyam2 0:022cc499efd8 44 #include "mbed.h"
wcyam2 0:022cc499efd8 45 #define isOn(a) a<0.3f //Analog button pressed
wcyam2 0:022cc499efd8 46 AnalogIn min_post(A0);
wcyam2 0:022cc499efd8 47 AnalogIn minus_post(A1);
wcyam2 0:022cc499efd8 48 AnalogIn mid_post(A2);
wcyam2 0:022cc499efd8 49 AnalogIn plus_post(A3);
wcyam2 0:022cc499efd8 50 AnalogIn max_post(A4);
wcyam2 0:022cc499efd8 51 AnalogIn prev_post(A5);
wcyam2 0:022cc499efd8 52 InterruptIn mybutton(BUTTON1);
wcyam2 0:022cc499efd8 53 //servo output
wcyam2 0:022cc499efd8 54 PwmOut servo(D3);
wcyam2 0:022cc499efd8 55
wcyam2 0:022cc499efd8 56 //debug light
wcyam2 0:022cc499efd8 57 DigitalOut led(PA_5);
wcyam2 0:022cc499efd8 58
wcyam2 0:022cc499efd8 59 //user setting
wcyam2 0:022cc499efd8 60 //hertz(hz) / period time(s) of servo
wcyam2 0:022cc499efd8 61 const double hertz = 50;
wcyam2 0:022cc499efd8 62 const double servo_period = 1/hertz;
wcyam2 0:022cc499efd8 63 //pulsewidth(us)
wcyam2 0:022cc499efd8 64 const double min_us = 500;
wcyam2 0:022cc499efd8 65 const double max_us = 2500;
wcyam2 0:022cc499efd8 66 //degree
wcyam2 0:022cc499efd8 67 const double min_degree = 0; //refer to the minimum pulsewidth(us)
wcyam2 0:022cc499efd8 68 const double max_degree = 180; //refer to the maximum pulsewidth(us)
wcyam2 0:022cc499efd8 69
wcyam2 0:022cc499efd8 70 //mode
wcyam2 0:022cc499efd8 71 const bool degree_mode = false; // 0: false, 1:true
wcyam2 0:022cc499efd8 72
wcyam2 0:022cc499efd8 73 //button setting for [+10%(A3)] and [-10%(A1)]
wcyam2 0:022cc499efd8 74 //change if degree_mode == true
wcyam2 0:022cc499efd8 75 const double change_degree = 1; //range (0.0, 180.0]
wcyam2 0:022cc499efd8 76 //change if degree_mode == false
wcyam2 0:022cc499efd8 77 const double change_percentage = 0.05; //range (0.0, 1.0)
wcyam2 0:022cc499efd8 78
wcyam2 0:022cc499efd8 79 //user setting end
wcyam2 0:022cc499efd8 80
wcyam2 0:022cc499efd8 81 //------------------------------------------------------------------------------------------------------------
wcyam2 0:022cc499efd8 82
wcyam2 0:022cc499efd8 83 //calculate variable
wcyam2 0:022cc499efd8 84 const double mid_us = (max_us + min_us)/2;
wcyam2 0:022cc499efd8 85 const double change = degree_mode?change_degree/(max_degree - min_degree)*(max_us - min_us)+ min_us:(max_us - min_us)*change_percentage;
wcyam2 0:022cc499efd8 86 int factor = 2;
wcyam2 0:022cc499efd8 87 bool reset = true;
wcyam2 0:022cc499efd8 88 //calculate variable end
wcyam2 0:022cc499efd8 89
wcyam2 0:022cc499efd8 90 // degree to pwm function
wcyam2 0:022cc499efd8 91 double degree2pwm(double d){
wcyam2 0:022cc499efd8 92 return (d/(max_degree - min_degree)*(max_us - min_us)+ min_us);
wcyam2 0:022cc499efd8 93 }
wcyam2 0:022cc499efd8 94
wcyam2 0:022cc499efd8 95 //initialize function
wcyam2 0:022cc499efd8 96 void blink_testing(double w){
wcyam2 0:022cc499efd8 97 led = 0;
wcyam2 0:022cc499efd8 98 for(int i = 1; i <= factor; i++){
wcyam2 0:022cc499efd8 99 led = 1; wait(w);
wcyam2 0:022cc499efd8 100 led = 0; wait(w);
wcyam2 0:022cc499efd8 101 }
wcyam2 0:022cc499efd8 102 }
wcyam2 0:022cc499efd8 103
wcyam2 0:022cc499efd8 104 void m(){
wcyam2 0:022cc499efd8 105 // [input] button design
wcyam2 0:022cc499efd8 106 // #after start
wcyam2 0:022cc499efd8 107 // A5 A4 A3 A2 A1 A0
wcyam2 0:022cc499efd8 108 // prev max +% mid -% min
wcyam2 0:022cc499efd8 109 //set previous, current and next pulsewidth status
wcyam2 0:022cc499efd8 110 double prev_pulsewidth = mid_us;
wcyam2 0:022cc499efd8 111 double curr_pulsewidth = mid_us;
wcyam2 0:022cc499efd8 112 double next_pulsewidth = mid_us;
wcyam2 0:022cc499efd8 113 while(reset){
wcyam2 0:022cc499efd8 114 //button control (change: next_pulsewidth)
wcyam2 0:022cc499efd8 115 if (isOn(mid_post)) next_pulsewidth = mid_us;
wcyam2 0:022cc499efd8 116 else if (isOn(min_post)) next_pulsewidth = min_us;
wcyam2 0:022cc499efd8 117 else if (isOn(max_post)) next_pulsewidth = max_us;
wcyam2 0:022cc499efd8 118
wcyam2 0:022cc499efd8 119 else if (isOn(minus_post)){
wcyam2 0:022cc499efd8 120 //wait until release button
wcyam2 0:022cc499efd8 121 while(isOn(minus_post)); next_pulsewidth = max(min_us, next_pulsewidth - change*factor);
wcyam2 0:022cc499efd8 122 }
wcyam2 0:022cc499efd8 123 else if (isOn(plus_post)){
wcyam2 0:022cc499efd8 124 //wait until release button
wcyam2 0:022cc499efd8 125 while(isOn(plus_post)); next_pulsewidth = min(max_us, next_pulsewidth + change*factor);
wcyam2 0:022cc499efd8 126 }
wcyam2 0:022cc499efd8 127 else if (isOn(prev_post)){
wcyam2 0:022cc499efd8 128 //action immediately
wcyam2 0:022cc499efd8 129 servo.pulsewidth_us(prev_pulsewidth);
wcyam2 0:022cc499efd8 130 next_pulsewidth = prev_pulsewidth;
wcyam2 0:022cc499efd8 131 //wait until release button
wcyam2 0:022cc499efd8 132 while(isOn(plus_post));
wcyam2 0:022cc499efd8 133 }
wcyam2 0:022cc499efd8 134 //button control end
wcyam2 0:022cc499efd8 135
wcyam2 0:022cc499efd8 136 //action if button change exclude [prev_post(A5)]
wcyam2 0:022cc499efd8 137 if (next_pulsewidth != curr_pulsewidth){
wcyam2 0:022cc499efd8 138 servo.pulsewidth_us(next_pulsewidth);
wcyam2 0:022cc499efd8 139 prev_pulsewidth = curr_pulsewidth;
wcyam2 0:022cc499efd8 140 curr_pulsewidth = next_pulsewidth;
wcyam2 0:022cc499efd8 141 led = !led;
wcyam2 0:022cc499efd8 142 //wait(0.2f);
wcyam2 0:022cc499efd8 143 }
wcyam2 0:022cc499efd8 144 }
wcyam2 0:022cc499efd8 145 }
wcyam2 0:022cc499efd8 146 void restart(){
wcyam2 0:022cc499efd8 147 reset = false;
wcyam2 0:022cc499efd8 148 led = 0; wait (0.2f);
wcyam2 0:022cc499efd8 149 blink_testing(0.2f);
wcyam2 0:022cc499efd8 150 }
wcyam2 0:022cc499efd8 151 int main() {
wcyam2 0:022cc499efd8 152 //initialize
wcyam2 0:022cc499efd8 153 mybutton.rise(&restart);
wcyam2 0:022cc499efd8 154 blink_testing(0.5f); //blink onboard led 2 second
wcyam2 0:022cc499efd8 155 servo.period(servo_period); //set period time of servo
wcyam2 0:022cc499efd8 156 servo.pulsewidth_us(mid_us); //write the pulsewidth (init: mid)
wcyam2 0:022cc499efd8 157 factor = 1;
wcyam2 0:022cc499efd8 158 while(1){
wcyam2 0:022cc499efd8 159 // [input] button design
wcyam2 0:022cc499efd8 160 // A5 A4 A3 A2 A1 A0
wcyam2 0:022cc499efd8 161 // start read +1 reset -1 read
wcyam2 0:022cc499efd8 162 if (isOn(mid_post)) factor = 1;
wcyam2 0:022cc499efd8 163 else if (isOn(minus_post)){
wcyam2 0:022cc499efd8 164 //wait until release button
wcyam2 0:022cc499efd8 165 while(isOn(minus_post)); factor = max(1, factor-1);
wcyam2 0:022cc499efd8 166 }
wcyam2 0:022cc499efd8 167 else if (isOn(plus_post)){
wcyam2 0:022cc499efd8 168 //wait until release button
wcyam2 0:022cc499efd8 169 while(isOn(plus_post)); factor = min(10, factor+1);
wcyam2 0:022cc499efd8 170 }
wcyam2 0:022cc499efd8 171 else if (isOn(max_post) || isOn(min_post)){
wcyam2 0:022cc499efd8 172 //wait until release button
wcyam2 0:022cc499efd8 173 while(isOn(max_post) || isOn(min_post));
wcyam2 0:022cc499efd8 174 blink_testing(0.2f);
wcyam2 0:022cc499efd8 175 }
wcyam2 0:022cc499efd8 176 else if (isOn(prev_post)){
wcyam2 0:022cc499efd8 177 //wait until release button
wcyam2 0:022cc499efd8 178 while(isOn(plus_post)); m(); //start
wcyam2 0:022cc499efd8 179 }
wcyam2 0:022cc499efd8 180 reset = true;
wcyam2 0:022cc499efd8 181
wcyam2 0:022cc499efd8 182 }
wcyam2 0:022cc499efd8 183
wcyam2 0:022cc499efd8 184 }