ryo ikezawa / Mbed 2 deprecated mbed_blinky

n

Dependencies:   mbed

main.cpp@1:fbed80ea9cf9, 2018-06-19 (annotated)

Committer:
kavhirow
Date:
Tue Jun 19 07:34:39 2018 +0000
Revision:
1:fbed80ea9cf9
Parent:
0:a3c0b8cd6c61 
last ver

;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kavhirow 0:a3c0b8cd6c61 1 #include "mbed.h"
kavhirow 0:a3c0b8cd6c61 2
kavhirow 0:a3c0b8cd6c61 3 // Sampling frequency
kavhirow 0:a3c0b8cd6c61 4 #define SAMP_FREQ 2000.0
kavhirow 0:a3c0b8cd6c61 5 // Motor voltage limit (V_motor = 3.3 * v_out * 4)
kavhirow 0:a3c0b8cd6c61 6 #define VLIMIT 0.1
kavhirow 0:a3c0b8cd6c61 7 // Motor offset voltage
kavhirow 0:a3c0b8cd6c61 8 #define VOFFSET 0.0
kavhirow 0:a3c0b8cd6c61 9 // Feedback gains (Arbitrary Unit)
kavhirow 0:a3c0b8cd6c61 10 #define KP 1.0
kavhirow 0:a3c0b8cd6c61 11 #define KD 0.04
kavhirow 1:fbed80ea9cf9 12 #define KX -0.001
kavhirow 1:fbed80ea9cf9 13 #define KV -0.0007
kavhirow 0:a3c0b8cd6c61 14
kavhirow 0:a3c0b8cd6c61 15
kavhirow 0:a3c0b8cd6c61 16 // Define LEDs
kavhirow 0:a3c0b8cd6c61 17 PwmOut led_blink(LED1); // Blink at 1 Hz during operation
kavhirow 0:a3c0b8cd6c61 18 PwmOut led_v_over(LED2); // Indicate voltage saturation
kavhirow 0:a3c0b8cd6c61 19 PwmOut led_v_forward(LED3); // Indicate voltage for forward motion
kavhirow 0:a3c0b8cd6c61 20 PwmOut led_v_backward(LED4); // Indicate voltage for backward motion
kavhirow 0:a3c0b8cd6c61 21
kavhirow 0:a3c0b8cd6c61 22 // Define motor controls
kavhirow 0:a3c0b8cd6c61 23 DigitalOut motor_f(p13); // Motor forward
kavhirow 0:a3c0b8cd6c61 24 DigitalOut motor_r(p14); // Motor backward
kavhirow 0:a3c0b8cd6c61 25 AnalogOut motor_v(p18); // Motor driving voltage
kavhirow 0:a3c0b8cd6c61 26
kavhirow 0:a3c0b8cd6c61 27 // Define Photo-diode readings for angle sensor
kavhirow 0:a3c0b8cd6c61 28 AnalogIn pd1(p19);
kavhirow 0:a3c0b8cd6c61 29 AnalogIn pd2(p20);
kavhirow 0:a3c0b8cd6c61 30
kavhirow 0:a3c0b8cd6c61 31 // Define Photo-diode readings for encoder
kavhirow 0:a3c0b8cd6c61 32 AnalogIn pd3(p15);
kavhirow 0:a3c0b8cd6c61 33 AnalogIn pd4(p16);
kavhirow 0:a3c0b8cd6c61 34
kavhirow 0:a3c0b8cd6c61 35 Serial pc(USBTX, USBRX); // Serial communication
kavhirow 0:a3c0b8cd6c61 36 Ticker t_int; // Interrupt ticker
kavhirow 0:a3c0b8cd6c61 37
kavhirow 0:a3c0b8cd6c61 38 float brightness = 0; // Brightness for blinking led_blink
kavhirow 0:a3c0b8cd6c61 39 float br_inc = 1.0/SAMP_FREQ; // For blinkng LED at 1 Hz
kavhirow 0:a3c0b8cd6c61 40 float angle, angle_prev = 0; // Angle and Previous angle (for calculating omega)
kavhirow 0:a3c0b8cd6c61 41 float v_out; // Output voltage
kavhirow 0:a3c0b8cd6c61 42 float v_dir; // Voltage polarity
kavhirow 0:a3c0b8cd6c61 43 float omega = 0; // Angluar velocity
kavhirow 0:a3c0b8cd6c61 44 float f_omega = 0; // Filtered angluar velocity
kavhirow 0:a3c0b8cd6c61 45 int value1 = 0;
kavhirow 0:a3c0b8cd6c61 46 int value2 = 0;
kavhirow 0:a3c0b8cd6c61 47 int value1_prev = 0;
kavhirow 0:a3c0b8cd6c61 48 int value2_prev = 0;
kavhirow 0:a3c0b8cd6c61 49 int count = 0;
kavhirow 0:a3c0b8cd6c61 50 int count_prev = 0;
kavhirow 0:a3c0b8cd6c61 51 float velocity = 0;
kavhirow 0:a3c0b8cd6c61 52 float velocity_prev = 0;
kavhirow 0:a3c0b8cd6c61 53 float velocity_prev_prev = 0;
kavhirow 0:a3c0b8cd6c61 54 float f_velocity = 0;
kavhirow 0:a3c0b8cd6c61 55 float f_velocity_prev = 0;
kavhirow 0:a3c0b8cd6c61 56 float f_velocity_prev_prev = 0;
kavhirow 0:a3c0b8cd6c61 57 float omega_prev = 0;
kavhirow 0:a3c0b8cd6c61 58 float f_omega_prev = 0;
kavhirow 1:fbed80ea9cf9 59 float ref = 0;
kavhirow 0:a3c0b8cd6c61 60
kavhirow 0:a3c0b8cd6c61 61 // Interrupt handler (control loop)
kavhirow 0:a3c0b8cd6c61 62 void int0() {
kavhirow 0:a3c0b8cd6c61 63 // read difference of photo-diodes
kavhirow 0:a3c0b8cd6c61 64 angle = pd1 - pd2;
kavhirow 0:a3c0b8cd6c61 65
kavhirow 0:a3c0b8cd6c61 66 //binal
kavhirow 0:a3c0b8cd6c61 67 if (pd3 > 0.65) {
kavhirow 0:a3c0b8cd6c61 68 value1 = 1;
kavhirow 0:a3c0b8cd6c61 69 } else {
kavhirow 0:a3c0b8cd6c61 70 value1 = 0;
kavhirow 0:a3c0b8cd6c61 71 }
kavhirow 0:a3c0b8cd6c61 72 if (pd4 > 0.72) {
kavhirow 0:a3c0b8cd6c61 73 value2 = 1;
kavhirow 0:a3c0b8cd6c61 74 } else {
kavhirow 0:a3c0b8cd6c61 75 value2 = 0;
kavhirow 0:a3c0b8cd6c61 76 }
kavhirow 0:a3c0b8cd6c61 77 //keisoku
kavhirow 0:a3c0b8cd6c61 78 if(value1 != value1_prev){
kavhirow 0:a3c0b8cd6c61 79 //rising
kavhirow 0:a3c0b8cd6c61 80 if(value1 == 1){
kavhirow 0:a3c0b8cd6c61 81 if(value2 == 1){
kavhirow 0:a3c0b8cd6c61 82 count += 1;
kavhirow 0:a3c0b8cd6c61 83 } else {
kavhirow 0:a3c0b8cd6c61 84 count -= 1;
kavhirow 0:a3c0b8cd6c61 85 }
kavhirow 0:a3c0b8cd6c61 86 //falling
kavhirow 0:a3c0b8cd6c61 87 } else {
kavhirow 0:a3c0b8cd6c61 88 if(value2 ==1){
kavhirow 0:a3c0b8cd6c61 89 count -= 1;
kavhirow 0:a3c0b8cd6c61 90 } else {
kavhirow 0:a3c0b8cd6c61 91 count += 1;
kavhirow 0:a3c0b8cd6c61 92 }
kavhirow 0:a3c0b8cd6c61 93 }
kavhirow 0:a3c0b8cd6c61 94 }
kavhirow 0:a3c0b8cd6c61 95 if(value2 != value2_prev){
kavhirow 0:a3c0b8cd6c61 96 //rising
kavhirow 0:a3c0b8cd6c61 97 if(value2 == 1){
kavhirow 0:a3c0b8cd6c61 98 if(value1 == 1){
kavhirow 0:a3c0b8cd6c61 99 count -= 1;
kavhirow 0:a3c0b8cd6c61 100 } else {
kavhirow 0:a3c0b8cd6c61 101 count += 1;
kavhirow 0:a3c0b8cd6c61 102 }
kavhirow 0:a3c0b8cd6c61 103 //falling
kavhirow 0:a3c0b8cd6c61 104 } else {
kavhirow 0:a3c0b8cd6c61 105 if(value1 ==1){
kavhirow 0:a3c0b8cd6c61 106 count += 1;
kavhirow 0:a3c0b8cd6c61 107 } else {
kavhirow 0:a3c0b8cd6c61 108 count -= 1;
kavhirow 0:a3c0b8cd6c61 109 }
kavhirow 0:a3c0b8cd6c61 110 }
kavhirow 0:a3c0b8cd6c61 111 }
kavhirow 0:a3c0b8cd6c61 112
kavhirow 0:a3c0b8cd6c61 113 // calculate omega
kavhirow 0:a3c0b8cd6c61 114 omega = (angle - angle_prev) * SAMP_FREQ;
kavhirow 0:a3c0b8cd6c61 115 angle_prev = angle;
kavhirow 0:a3c0b8cd6c61 116 // filtering omega
kavhirow 0:a3c0b8cd6c61 117 f_omega = 0.0155*omega + 0.0155*omega_prev + 0.9691*f_omega_prev;
kavhirow 0:a3c0b8cd6c61 118 omega_prev = omega;
kavhirow 0:a3c0b8cd6c61 119 f_omega_prev = f_omega;
kavhirow 0:a3c0b8cd6c61 120
kavhirow 0:a3c0b8cd6c61 121 // calculate velocity
kavhirow 0:a3c0b8cd6c61 122 velocity = (count - count_prev) * SAMP_FREQ;
kavhirow 0:a3c0b8cd6c61 123 // filtering velocity
kavhirow 0:a3c0b8cd6c61 124 f_velocity = 0.0002414*velocity + 0.0004827*velocity_prev + 0.0002414*velocity_prev_prev + 1.9556*f_velocity_prev - 0.9565*f_velocity_prev_prev;
kavhirow 0:a3c0b8cd6c61 125
kavhirow 0:a3c0b8cd6c61 126 //update value
kavhirow 1:fbed80ea9cf9 127 velocity_prev_prev = velocity_prev;
kavhirow 0:a3c0b8cd6c61 128 velocity_prev = velocity;
kavhirow 0:a3c0b8cd6c61 129
kavhirow 1:fbed80ea9cf9 130 f_velocity_prev_prev = f_velocity_prev;
kavhirow 0:a3c0b8cd6c61 131 f_velocity_prev = f_velocity;
kavhirow 0:a3c0b8cd6c61 132
kavhirow 0:a3c0b8cd6c61 133 count_prev = count;
kavhirow 0:a3c0b8cd6c61 134 value1_prev = value1;
kavhirow 0:a3c0b8cd6c61 135 value2_prev = value2;
kavhirow 0:a3c0b8cd6c61 136
kavhirow 0:a3c0b8cd6c61 137
kavhirow 0:a3c0b8cd6c61 138 // output
kavhirow 1:fbed80ea9cf9 139 v_out = angle * KP + f_omega * KD + (count - ref) * KX + f_velocity * KV;
kavhirow 1:fbed80ea9cf9 140 ref += 40.0 / 2000.0;
kavhirow 0:a3c0b8cd6c61 141 //v_out = count * KX + f_velocity * KV;
kavhirow 0:a3c0b8cd6c61 142 //v_out = angle * KP + f_omega * KD ;
kavhirow 0:a3c0b8cd6c61 143 if (v_out > 0) {
kavhirow 0:a3c0b8cd6c61 144 motor_f = 1;
kavhirow 0:a3c0b8cd6c61 145 motor_r = 0;
kavhirow 0:a3c0b8cd6c61 146 v_dir = 1.0;
kavhirow 0:a3c0b8cd6c61 147 } else {
kavhirow 0:a3c0b8cd6c61 148 motor_f = 0;
kavhirow 0:a3c0b8cd6c61 149 motor_r = 1;
kavhirow 0:a3c0b8cd6c61 150 v_out *= -1.0;
kavhirow 0:a3c0b8cd6c61 151 v_dir = -1.0;
kavhirow 0:a3c0b8cd6c61 152 }
kavhirow 0:a3c0b8cd6c61 153 if (v_out > VLIMIT) {
kavhirow 0:a3c0b8cd6c61 154 v_out = VLIMIT;
kavhirow 0:a3c0b8cd6c61 155 led_v_over = 1;
kavhirow 0:a3c0b8cd6c61 156 } else {
kavhirow 0:a3c0b8cd6c61 157 led_v_over = 0;
kavhirow 0:a3c0b8cd6c61 158 }
kavhirow 0:a3c0b8cd6c61 159 // Motor output
kavhirow 0:a3c0b8cd6c61 160 if (pd1 < 0.02 && pd2 < 0.02) { // if not standing, stop the motor
kavhirow 0:a3c0b8cd6c61 161 motor_v = 0;
kavhirow 0:a3c0b8cd6c61 162 led_v_forward = 1;
kavhirow 0:a3c0b8cd6c61 163 led_v_backward = 1;
kavhirow 0:a3c0b8cd6c61 164 } else {
kavhirow 0:a3c0b8cd6c61 165 motor_v = v_out + VOFFSET;
kavhirow 0:a3c0b8cd6c61 166 led_v_forward = (v_dir > 0 ? v_out*3.3 : 0);
kavhirow 0:a3c0b8cd6c61 167 led_v_backward = (v_dir < 0 ? v_out*3.3 : 0);
kavhirow 0:a3c0b8cd6c61 168 }
kavhirow 0:a3c0b8cd6c61 169 // blink LED
kavhirow 0:a3c0b8cd6c61 170 brightness += br_inc;
kavhirow 0:a3c0b8cd6c61 171 if (brightness >= 1) brightness = 0;
kavhirow 0:a3c0b8cd6c61 172 led_blink = brightness;
kavhirow 0:a3c0b8cd6c61 173
kavhirow 0:a3c0b8cd6c61 174 // send 6 data to PC (data should be in [0,255]. Data must be casted into (char) type)
kavhirow 0:a3c0b8cd6c61 175 //pc.putc((char)(pd1 * 128 + 128));
kavhirow 0:a3c0b8cd6c61 176 //pc.putc((char)(pd2 * 128 + 128));
kavhirow 0:a3c0b8cd6c61 177 pc.putc((char)(f_velocity + 128));
kavhirow 0:a3c0b8cd6c61 178 pc.putc((char)(count + 128));
kavhirow 0:a3c0b8cd6c61 179 pc.putc((char)(velocity + 128));
kavhirow 0:a3c0b8cd6c61 180 //pc.putc((char)(angle * 128 + 128));
kavhirow 0:a3c0b8cd6c61 181 //pc.putc((char)(value1 * 128 + 100));
kavhirow 0:a3c0b8cd6c61 182 //pc.putc((char)(value2 * 128 + 120));
kavhirow 0:a3c0b8cd6c61 183 //pc.putc((char)(omega * 10 + 128));
kavhirow 0:a3c0b8cd6c61 184 //pc.putc((char)(f_omega * 10 + 128));
kavhirow 0:a3c0b8cd6c61 185 pc.putc((char)(count + 128));
kavhirow 0:a3c0b8cd6c61 186 pc.putc((char)(f_velocity + 128));
kavhirow 0:a3c0b8cd6c61 187 pc.putc((char)(v_out * 100 + 128));
kavhirow 0:a3c0b8cd6c61 188
kavhirow 0:a3c0b8cd6c61 189 }
kavhirow 0:a3c0b8cd6c61 190
kavhirow 0:a3c0b8cd6c61 191
kavhirow 0:a3c0b8cd6c61 192 // Main function is only for initialization
kavhirow 0:a3c0b8cd6c61 193 int main() {
kavhirow 0:a3c0b8cd6c61 194 // Initializing LED ports and Serial port
kavhirow 0:a3c0b8cd6c61 195 led_blink.period(1.0/SAMP_FREQ);
kavhirow 0:a3c0b8cd6c61 196 led_v_over.period(1.0/SAMP_FREQ);
kavhirow 0:a3c0b8cd6c61 197 led_v_forward.period(1.0/SAMP_FREQ);
kavhirow 0:a3c0b8cd6c61 198 led_v_backward.period(1.0/SAMP_FREQ);
kavhirow 0:a3c0b8cd6c61 199
kavhirow 0:a3c0b8cd6c61 200 pc.baud(230400);
kavhirow 0:a3c0b8cd6c61 201
kavhirow 0:a3c0b8cd6c61 202 // Wait for 1 second before starting feedback control
kavhirow 0:a3c0b8cd6c61 203 wait(1);
kavhirow 0:a3c0b8cd6c61 204 pc.printf("12345678"); // For initializing Processing plotter program
kavhirow 0:a3c0b8cd6c61 205
kavhirow 0:a3c0b8cd6c61 206 // Start Control (to be stopped with any data from serial port)
kavhirow 0:a3c0b8cd6c61 207 t_int.attach(&int0, 1.0/SAMP_FREQ); // Start timer-interrupt
kavhirow 0:a3c0b8cd6c61 208 while(1) {
kavhirow 0:a3c0b8cd6c61 209 if (pc.readable() == 1) { // If any data in serial prot
kavhirow 0:a3c0b8cd6c61 210 pc.putc(pc.getc()); // Echo back the data
kavhirow 0:a3c0b8cd6c61 211 t_int.detach(); // Stop timer-interrupt
kavhirow 0:a3c0b8cd6c61 212 break;
kavhirow 0:a3c0b8cd6c61 213 }
kavhirow 0:a3c0b8cd6c61 214 }
kavhirow 0:a3c0b8cd6c61 215 }