Sven Kugathasan / Mbed OS SKAFMO_2

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:   PID

main.cpp@0:b6deec3905f4, 2017-03-24 (annotated)

Committer:
svenkugi
Date:
Fri Mar 24 00:34:17 2017 +0000
Revision:
0:b6deec3905f4
Child:
1:0eb5cc1bd38f
Child:
2:a14dc197218c
ES

Who changed what in which revision?

UserRevisionLine numberNew contents of line
svenkugi 0:b6deec3905f4 1 /*_________________________________LIBRARIES__________________________________*/
svenkugi 0:b6deec3905f4 2
svenkugi 0:b6deec3905f4 3 #include "mbed.h"
svenkugi 0:b6deec3905f4 4 #include "rtos.h"
svenkugi 0:b6deec3905f4 5 #include "PID.h"
svenkugi 0:b6deec3905f4 6 #include "Regexp.h"
svenkugi 0:b6deec3905f4 7 #include "string.h"
svenkugi 0:b6deec3905f4 8 #include "stdlib.h"
svenkugi 0:b6deec3905f4 9 #include <string>
svenkugi 0:b6deec3905f4 10 #include <ctype.h>
svenkugi 0:b6deec3905f4 11 #include <RawSerial.h>
svenkugi 0:b6deec3905f4 12
svenkugi 0:b6deec3905f4 13 /*_________________________________PIN SETUP__________________________________*/
svenkugi 0:b6deec3905f4 14
svenkugi 0:b6deec3905f4 15 //PhotoInterrupter Input Pins
svenkugi 0:b6deec3905f4 16 #define I1pin D2
svenkugi 0:b6deec3905f4 17 #define I2pin D11
svenkugi 0:b6deec3905f4 18 #define I3pin D12
svenkugi 0:b6deec3905f4 19
svenkugi 0:b6deec3905f4 20 //Incremental Encoder Input Pins
svenkugi 0:b6deec3905f4 21 #define CHA D7
svenkugi 0:b6deec3905f4 22 #define CHB D8
svenkugi 0:b6deec3905f4 23
svenkugi 0:b6deec3905f4 24 //Motor Drive output pins //Mask in output byte
svenkugi 0:b6deec3905f4 25 #define L1Lpin D4 //0x01
svenkugi 0:b6deec3905f4 26 #define L1Hpin D5 //0x02
svenkugi 0:b6deec3905f4 27 #define L2Lpin D3 //0x04
svenkugi 0:b6deec3905f4 28 #define L2Hpin D6 //0x08
svenkugi 0:b6deec3905f4 29 #define L3Lpin D9 //0x10
svenkugi 0:b6deec3905f4 30 #define L3Hpin D10 //0x20
svenkugi 0:b6deec3905f4 31
svenkugi 0:b6deec3905f4 32 //Photointerrupter Inputs as Interrupts
svenkugi 0:b6deec3905f4 33 InterruptIn InterruptI1(D2);
svenkugi 0:b6deec3905f4 34 InterruptIn InterruptI2(D11);
svenkugi 0:b6deec3905f4 35 InterruptIn InterruptI3(D12);
svenkugi 0:b6deec3905f4 36
svenkugi 0:b6deec3905f4 37 //Incremental Encoder Inputs as Interrupts
svenkugi 0:b6deec3905f4 38 InterruptIn InterruptCHA(D7);
svenkugi 0:b6deec3905f4 39 DigitalIn InterruptCHB(D8);
svenkugi 0:b6deec3905f4 40
svenkugi 0:b6deec3905f4 41 //Motor Drive Outputs in PWM
svenkugi 0:b6deec3905f4 42 PwmOut L1L(L1Lpin);
svenkugi 0:b6deec3905f4 43 PwmOut L1H(L1Hpin);
svenkugi 0:b6deec3905f4 44 PwmOut L2L(L2Lpin);
svenkugi 0:b6deec3905f4 45 PwmOut L2H(L2Hpin);
svenkugi 0:b6deec3905f4 46 PwmOut L3L(L3Lpin);
svenkugi 0:b6deec3905f4 47 PwmOut L3H(L3Hpin);
svenkugi 0:b6deec3905f4 48
svenkugi 0:b6deec3905f4 49 //Status LED
svenkugi 0:b6deec3905f4 50 DigitalOut led1(LED1);
svenkugi 0:b6deec3905f4 51
svenkugi 0:b6deec3905f4 52 //Initialise the serial port
svenkugi 0:b6deec3905f4 53 Serial pc(SERIAL_TX, SERIAL_RX);
svenkugi 0:b6deec3905f4 54
svenkugi 0:b6deec3905f4 55 string input;
svenkugi 0:b6deec3905f4 56
svenkugi 0:b6deec3905f4 57 int position_r;
svenkugi 0:b6deec3905f4 58 int position_v;
svenkugi 0:b6deec3905f4 59
svenkugi 0:b6deec3905f4 60 float r;
svenkugi 0:b6deec3905f4 61 float v;
svenkugi 0:b6deec3905f4 62 string note;
svenkugi 0:b6deec3905f4 63 int duration;
svenkugi 0:b6deec3905f4 64 int ind1=1;
svenkugi 0:b6deec3905f4 65 int ind2=0;
svenkugi 0:b6deec3905f4 66
svenkugi 0:b6deec3905f4 67 char temp [255];
svenkugi 0:b6deec3905f4 68
svenkugi 0:b6deec3905f4 69
svenkugi 0:b6deec3905f4 70 //Initialize Threads
svenkugi 0:b6deec3905f4 71 Thread thread;
svenkugi 0:b6deec3905f4 72
svenkugi 0:b6deec3905f4 73 //Timer
svenkugi 0:b6deec3905f4 74 Timer rps; // Measures Time for complete revolution
svenkugi 0:b6deec3905f4 75 Timer partial_rps; // Measures Time for partial revolutions
svenkugi 0:b6deec3905f4 76 Timer tmp; // Profiler Timer
svenkugi 0:b6deec3905f4 77
svenkugi 0:b6deec3905f4 78 //PID Controller
svenkugi 0:b6deec3905f4 79 PID velocity_pid(0.5, 0.5, 0.1, 0.05); // (P, I, D, WAIT)
svenkugi 0:b6deec3905f4 80 PID dist_pid(2, 0.0, 0.005, 0.05); // (P, I, D, WAIT)
svenkugi 0:b6deec3905f4 81
svenkugi 0:b6deec3905f4 82 /*________________________Motor Drive States__________________________________*/
svenkugi 0:b6deec3905f4 83
svenkugi 0:b6deec3905f4 84 //Mapping from sequential drive states to motor phase outputs
svenkugi 0:b6deec3905f4 85 /*
svenkugi 0:b6deec3905f4 86 State L1 L2 L3
svenkugi 0:b6deec3905f4 87 0 H - L
svenkugi 0:b6deec3905f4 88 1 - H L
svenkugi 0:b6deec3905f4 89 2 L H -
svenkugi 0:b6deec3905f4 90 3 L - H
svenkugi 0:b6deec3905f4 91 4 - L H
svenkugi 0:b6deec3905f4 92 5 H L -
svenkugi 0:b6deec3905f4 93 6 - - -
svenkugi 0:b6deec3905f4 94 7 - - -
svenkugi 0:b6deec3905f4 95 */
svenkugi 0:b6deec3905f4 96
svenkugi 0:b6deec3905f4 97 //Drive state to output table
svenkugi 0:b6deec3905f4 98 const int8_t driveTable[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00};
svenkugi 0:b6deec3905f4 99
svenkugi 0:b6deec3905f4 100 //Mapping from interrupter inputs to sequential rotor states. 0x00 and 0x07 are not valid
svenkugi 0:b6deec3905f4 101 const int8_t stateMap[] = {0x07,0x05,0x03,0x04,0x01,0x00,0x02,0x07};
svenkugi 0:b6deec3905f4 102
svenkugi 0:b6deec3905f4 103 //Alternative if phase order of input or drive is reversed
svenkugi 0:b6deec3905f4 104 //const int8_t stateMap_Reverse[] = {0x07,0x01,0x03,0x02,0x05,0x00,0x04,0x07};
svenkugi 0:b6deec3905f4 105
svenkugi 0:b6deec3905f4 106 /*____________________Global Variable Initialization__________________________*/
svenkugi 0:b6deec3905f4 107
svenkugi 0:b6deec3905f4 108 //Phase lead to make motor spin: 2 for forwards, -2 for backwards
svenkugi 0:b6deec3905f4 109 const int8_t lead = -2;
svenkugi 0:b6deec3905f4 110 int8_t direction = 1; //Default Setting: Backwards rotation
svenkugi 0:b6deec3905f4 111
svenkugi 0:b6deec3905f4 112 //Rotor Offset
svenkugi 0:b6deec3905f4 113 uint8_t orState=0;
svenkugi 0:b6deec3905f4 114
svenkugi 0:b6deec3905f4 115 //Disk State
svenkugi 0:b6deec3905f4 116 uint8_t intState=0; //Current Optical Disk state
svenkugi 0:b6deec3905f4 117
svenkugi 0:b6deec3905f4 118 //State Variables
svenkugi 0:b6deec3905f4 119 const uint8_t num_states = 6; //Number of states in one rotation
svenkugi 0:b6deec3905f4 120 uint32_t count = 0; //Counts number of states traversed
svenkugi 0:b6deec3905f4 121 int8_t completed = 0; //Checks if rotation completed
svenkugi 0:b6deec3905f4 122 int8_t driveto = 0; //Holds value of new motor drive state
svenkugi 0:b6deec3905f4 123
svenkugi 0:b6deec3905f4 124 //Debug Variables
svenkugi 0:b6deec3905f4 125 bool flag = false;
svenkugi 0:b6deec3905f4 126 float test_time=0;
svenkugi 0:b6deec3905f4 127 uint8_t test = 0;
svenkugi 0:b6deec3905f4 128
svenkugi 0:b6deec3905f4 129 //Angular Velocity Variables
svenkugi 0:b6deec3905f4 130 float period = 0; //Time taken to complete one rotation
svenkugi 0:b6deec3905f4 131 float angular_vel = 0; //Revolution per second
svenkugi 0:b6deec3905f4 132 float test_vel = 0;
svenkugi 0:b6deec3905f4 133
svenkugi 0:b6deec3905f4 134 float PWM_freq = 0.001f; //500Hz (> Motor LP cut-off frequency = 10Hz)
svenkugi 0:b6deec3905f4 135 float dutyout = 1.0f; //Initialized at 50% duty cycle
svenkugi 0:b6deec3905f4 136 float dutyout_max = 1.0f; //Maximum Duty Cycle
svenkugi 0:b6deec3905f4 137
svenkugi 0:b6deec3905f4 138 //float velocity_set = 10.0;
svenkugi 0:b6deec3905f4 139 uint32_t revstates_count = 0; //Global Variable to pass into interrupt
svenkugi 0:b6deec3905f4 140 float rev_target = 0.0f;
svenkugi 0:b6deec3905f4 141 float vel_target = 10.0f;
svenkugi 0:b6deec3905f4 142
svenkugi 0:b6deec3905f4 143 uint8_t pulse_count = 0;
svenkugi 0:b6deec3905f4 144 float total_rev = 0.0f;
svenkugi 0:b6deec3905f4 145 float partial_rev = 0.0f;
svenkugi 0:b6deec3905f4 146 float drive_vel = 0.0f;
svenkugi 0:b6deec3905f4 147
svenkugi 0:b6deec3905f4 148
svenkugi 0:b6deec3905f4 149 /*_____Basic Functions (Motor Drive, Synchronization, Reading Rotor State)____*/
svenkugi 0:b6deec3905f4 150
svenkugi 0:b6deec3905f4 151 //Set a given drive state
svenkugi 0:b6deec3905f4 152 void motorOut(int8_t driveState){
svenkugi 0:b6deec3905f4 153
svenkugi 0:b6deec3905f4 154 //Lookup the output byte from the drive state.
svenkugi 0:b6deec3905f4 155 int8_t driveOut = driveTable[driveState & 0x07];
svenkugi 0:b6deec3905f4 156
svenkugi 0:b6deec3905f4 157 //Turn off first (PWM)
svenkugi 0:b6deec3905f4 158 if (~driveOut & 0x01) L1L = 0;
svenkugi 0:b6deec3905f4 159 if (~driveOut & 0x02) L1H.write(dutyout); L1H.period(PWM_freq);
svenkugi 0:b6deec3905f4 160 if (~driveOut & 0x04) L2L = 0;
svenkugi 0:b6deec3905f4 161 if (~driveOut & 0x08) L2H.write(dutyout); L2H.period(PWM_freq);
svenkugi 0:b6deec3905f4 162 if (~driveOut & 0x10) L3L = 0;
svenkugi 0:b6deec3905f4 163 if (~driveOut & 0x20) L3H.write(dutyout); L3H.period(PWM_freq);
svenkugi 0:b6deec3905f4 164
svenkugi 0:b6deec3905f4 165 //Then turn on (PWM)
svenkugi 0:b6deec3905f4 166 if (driveOut & 0x01) L1L.write(dutyout); L1L.period(PWM_freq);
svenkugi 0:b6deec3905f4 167 if (driveOut & 0x02) L1H = 0;
svenkugi 0:b6deec3905f4 168 if (driveOut & 0x04) L2L.write(dutyout); L2L.period(PWM_freq);
svenkugi 0:b6deec3905f4 169 if (driveOut & 0x08) L2H = 0;
svenkugi 0:b6deec3905f4 170 if (driveOut & 0x10) L3L.write(dutyout); L3L.period(PWM_freq);
svenkugi 0:b6deec3905f4 171 if (driveOut & 0x20) L3H = 0;
svenkugi 0:b6deec3905f4 172
svenkugi 0:b6deec3905f4 173 //Turn off first
svenkugi 0:b6deec3905f4 174 //if (~driveOut & 0x01) L1L = 0;
svenkugi 0:b6deec3905f4 175 //if (~driveOut & 0x02) L1H = 1;
svenkugi 0:b6deec3905f4 176 //if (~driveOut & 0x04) L2L = 0;
svenkugi 0:b6deec3905f4 177 //if (~driveOut & 0x08) L2H = 1;
svenkugi 0:b6deec3905f4 178 //if (~driveOut & 0x10) L3L = 0;
svenkugi 0:b6deec3905f4 179 //if (~driveOut & 0x20) L3H = 1;
svenkugi 0:b6deec3905f4 180
svenkugi 0:b6deec3905f4 181 //Then turn on
svenkugi 0:b6deec3905f4 182 //if (driveOut & 0x01) L1L = 1;
svenkugi 0:b6deec3905f4 183 //if (driveOut & 0x02) L1H = 0;
svenkugi 0:b6deec3905f4 184 //if (driveOut & 0x04) L2L = 1;
svenkugi 0:b6deec3905f4 185 //if (driveOut & 0x08) L2H = 0;
svenkugi 0:b6deec3905f4 186 //if (driveOut & 0x10) L3L = 1;
svenkugi 0:b6deec3905f4 187 //if (driveOut & 0x20) L3H = 0;
svenkugi 0:b6deec3905f4 188 }
svenkugi 0:b6deec3905f4 189
svenkugi 0:b6deec3905f4 190 //Convert photointerrupter inputs to a rotor state
svenkugi 0:b6deec3905f4 191 inline int8_t readRotorState(){
svenkugi 0:b6deec3905f4 192 return stateMap[InterruptI1.read() + 2*InterruptI2.read() + 4*InterruptI3.read()];
svenkugi 0:b6deec3905f4 193 }
svenkugi 0:b6deec3905f4 194
svenkugi 0:b6deec3905f4 195 //Basic synchronisation routine
svenkugi 0:b6deec3905f4 196 int8_t motorHome() {
svenkugi 0:b6deec3905f4 197 dutyout = 1.0f;
svenkugi 0:b6deec3905f4 198
svenkugi 0:b6deec3905f4 199 //Put the motor in drive state X (e.g.5) tp avoid initial jittevbo
svenkugi 0:b6deec3905f4 200 motorOut(5);
svenkugi 0:b6deec3905f4 201 wait(1.0);
svenkugi 0:b6deec3905f4 202
svenkugi 0:b6deec3905f4 203 //Put the motor in drive state 0 and wait for it to stabilise
svenkugi 0:b6deec3905f4 204 motorOut(0);
svenkugi 0:b6deec3905f4 205 wait(1.0);
svenkugi 0:b6deec3905f4 206
svenkugi 0:b6deec3905f4 207 //Get the rotor state
svenkugi 0:b6deec3905f4 208 return readRotorState();
svenkugi 0:b6deec3905f4 209 }
svenkugi 0:b6deec3905f4 210
svenkugi 0:b6deec3905f4 211 /*________________Advanced Functions (Speed and Position Control)_____________*/
svenkugi 0:b6deec3905f4 212
svenkugi 0:b6deec3905f4 213 // Function has no PID
svenkugi 0:b6deec3905f4 214 void rotation_control(int8_t num_revs, int8_t sign){
svenkugi 0:b6deec3905f4 215
svenkugi 0:b6deec3905f4 216 revstates_count = num_revs*num_states;
svenkugi 0:b6deec3905f4 217
svenkugi 0:b6deec3905f4 218 intState = readRotorState();
svenkugi 0:b6deec3905f4 219 driveto = (intState-orState+(sign*lead)+6)%6;
svenkugi 0:b6deec3905f4 220 motorOut(driveto);
svenkugi 0:b6deec3905f4 221
svenkugi 0:b6deec3905f4 222 while(!completed){
svenkugi 0:b6deec3905f4 223
svenkugi 0:b6deec3905f4 224 //pc.printf("Angular velocity: %f \n", angular_vel);
svenkugi 0:b6deec3905f4 225 pc.printf("Partial Angular: %f \n", test_vel);
svenkugi 0:b6deec3905f4 226 //pc.printf("Count: %d \r\n", (count/6));
svenkugi 0:b6deec3905f4 227
svenkugi 0:b6deec3905f4 228 }
svenkugi 0:b6deec3905f4 229 }
svenkugi 0:b6deec3905f4 230
svenkugi 0:b6deec3905f4 231 // Funciton involves PID
svenkugi 0:b6deec3905f4 232 void position_control(float num_revs, int8_t sign, float vel_target){
svenkugi 0:b6deec3905f4 233
svenkugi 0:b6deec3905f4 234 rev_target = num_revs;
svenkugi 0:b6deec3905f4 235
svenkugi 0:b6deec3905f4 236 velocity_pid.setInputLimits(0.0, 50.0);
svenkugi 0:b6deec3905f4 237 velocity_pid.setOutputLimits(0.0, 1.0);
svenkugi 0:b6deec3905f4 238 velocity_pid.setMode(1);
svenkugi 0:b6deec3905f4 239 velocity_pid.setSetPoint(vel_target);
svenkugi 0:b6deec3905f4 240
svenkugi 0:b6deec3905f4 241 dist_pid.setInputLimits(0.0, 1000.0);
svenkugi 0:b6deec3905f4 242 dist_pid.setOutputLimits(0.2, 1.0);
svenkugi 0:b6deec3905f4 243 dist_pid.setMode(1);
svenkugi 0:b6deec3905f4 244 dist_pid.setSetPoint(rev_target);
svenkugi 0:b6deec3905f4 245
svenkugi 0:b6deec3905f4 246 //dist_pid.setInputLimits(0.0, 100.0);
svenkugi 0:b6deec3905f4 247 //dist_pid.setOutputLimits(0.2, 1.0);
svenkugi 0:b6deec3905f4 248 //dist_pid.setMode(1);
svenkugi 0:b6deec3905f4 249 //dist_pid.setSetPoint(num_revs);
svenkugi 0:b6deec3905f4 250
svenkugi 0:b6deec3905f4 251 intState = readRotorState();
svenkugi 0:b6deec3905f4 252 driveto = (intState-orState+(sign*lead)+6)%6;
svenkugi 0:b6deec3905f4 253 motorOut(driveto);
svenkugi 0:b6deec3905f4 254
svenkugi 0:b6deec3905f4 255 while(!completed){
svenkugi 0:b6deec3905f4 256
svenkugi 0:b6deec3905f4 257 //pc.printf("dutyout: %f \r\n", dutyout);
svenkugi 0:b6deec3905f4 258 pc.printf("Angular velocity: %f \r\n", angular_vel);
svenkugi 0:b6deec3905f4 259 pc.printf("Count: %d \r\n", count);
svenkugi 0:b6deec3905f4 260 //pc.printf("Total_rev: %f \r\n", total_rev);
svenkugi 0:b6deec3905f4 261 //pc.printf("\n");
svenkugi 0:b6deec3905f4 262 wait(0.05);
svenkugi 0:b6deec3905f4 263 }
svenkugi 0:b6deec3905f4 264
svenkugi 0:b6deec3905f4 265 }
svenkugi 0:b6deec3905f4 266
svenkugi 0:b6deec3905f4 267 void changestate_isr(){
svenkugi 0:b6deec3905f4 268
svenkugi 0:b6deec3905f4 269 // Profiling: Test time duration of ISR
svenkugi 0:b6deec3905f4 270 /*if(test == 0){
svenkugi 0:b6deec3905f4 271 tmp.start();
svenkugi 0:b6deec3905f4 272 test = 1;
svenkugi 0:b6deec3905f4 273 }
svenkugi 0:b6deec3905f4 274
svenkugi 0:b6deec3905f4 275 else{
svenkugi 0:b6deec3905f4 276 tmp.stop();
svenkugi 0:b6deec3905f4 277 test_time = tmp.read();
svenkugi 0:b6deec3905f4 278 tmp.reset();
svenkugi 0:b6deec3905f4 279 test = 0;
svenkugi 0:b6deec3905f4 280 }*/
svenkugi 0:b6deec3905f4 281
svenkugi 0:b6deec3905f4 282 // Measure time for 360 Rotation
svenkugi 0:b6deec3905f4 283 if(driveto == 0x04){ //Next time drivestate=4, 360 degrees revolution
svenkugi 0:b6deec3905f4 284 if(flag){
svenkugi 0:b6deec3905f4 285 rps.stop();
svenkugi 0:b6deec3905f4 286 angular_vel = 1/(rps.read());
svenkugi 0:b6deec3905f4 287 rps.reset();
svenkugi 0:b6deec3905f4 288 flag = 0;
svenkugi 0:b6deec3905f4 289 }
svenkugi 0:b6deec3905f4 290 }
svenkugi 0:b6deec3905f4 291
svenkugi 0:b6deec3905f4 292 if(driveto == 0x04){ //First time drivestate=4, Timer started at 0 degrees
svenkugi 0:b6deec3905f4 293 pulse_count = 0;
svenkugi 0:b6deec3905f4 294 rps.start();
svenkugi 0:b6deec3905f4 295 flag = 1;
svenkugi 0:b6deec3905f4 296 }
svenkugi 0:b6deec3905f4 297
svenkugi 0:b6deec3905f4 298 // Measure number of revolutions
svenkugi 0:b6deec3905f4 299 count++;
svenkugi 0:b6deec3905f4 300
svenkugi 0:b6deec3905f4 301 //Turn-off when target reached
svenkugi 0:b6deec3905f4 302 if(total_rev >= rev_target){
svenkugi 0:b6deec3905f4 303 completed = 1;
svenkugi 0:b6deec3905f4 304 dutyout = 0;
svenkugi 0:b6deec3905f4 305 motorOut(0);
svenkugi 0:b6deec3905f4 306 led1 = 0;
svenkugi 0:b6deec3905f4 307 __disable_irq();
svenkugi 0:b6deec3905f4 308 }
svenkugi 0:b6deec3905f4 309 else{
svenkugi 0:b6deec3905f4 310 intState = readRotorState();
svenkugi 0:b6deec3905f4 311 driveto = (intState-orState+direction*lead+6)%6;
svenkugi 0:b6deec3905f4 312 motorOut(driveto);
svenkugi 0:b6deec3905f4 313 }
svenkugi 0:b6deec3905f4 314
svenkugi 0:b6deec3905f4 315 }
svenkugi 0:b6deec3905f4 316
svenkugi 0:b6deec3905f4 317 void pid_isr(){
svenkugi 0:b6deec3905f4 318
svenkugi 0:b6deec3905f4 319 led1 = !led1;
svenkugi 0:b6deec3905f4 320 //tmp.start();
svenkugi 0:b6deec3905f4 321
svenkugi 0:b6deec3905f4 322 //117 Pulses per revolution
svenkugi 0:b6deec3905f4 323 pulse_count++;
svenkugi 0:b6deec3905f4 324
svenkugi 0:b6deec3905f4 325 //Measure Time to do 3 degrees of rotation
svenkugi 0:b6deec3905f4 326 if(test == 0){
svenkugi 0:b6deec3905f4 327 partial_rps.start();
svenkugi 0:b6deec3905f4 328 test = 1;
svenkugi 0:b6deec3905f4 329 }
svenkugi 0:b6deec3905f4 330 else{
svenkugi 0:b6deec3905f4 331 partial_rps.stop();
svenkugi 0:b6deec3905f4 332 test_vel = 1/((117.0f * partial_rps.read()));
svenkugi 0:b6deec3905f4 333 partial_rps.reset();
svenkugi 0:b6deec3905f4 334 test = 0;
svenkugi 0:b6deec3905f4 335 }
svenkugi 0:b6deec3905f4 336
svenkugi 0:b6deec3905f4 337 //Partial Revolution Count
svenkugi 0:b6deec3905f4 338 partial_rev = pulse_count/117.0f;
svenkugi 0:b6deec3905f4 339
svenkugi 0:b6deec3905f4 340 //Total Revolution Count
svenkugi 0:b6deec3905f4 341 total_rev = (count/6.0f) + partial_rev;
svenkugi 0:b6deec3905f4 342
svenkugi 0:b6deec3905f4 343 //Calculate new PID Control Point
svenkugi 0:b6deec3905f4 344 if((total_rev/rev_target) > 0.7f){
svenkugi 0:b6deec3905f4 345 dist_pid.setProcessValue(total_rev);
svenkugi 0:b6deec3905f4 346 dutyout = dist_pid.compute();
svenkugi 0:b6deec3905f4 347 }
svenkugi 0:b6deec3905f4 348 else{
svenkugi 0:b6deec3905f4 349 velocity_pid.setProcessValue(test_vel);
svenkugi 0:b6deec3905f4 350 dutyout = velocity_pid.compute();
svenkugi 0:b6deec3905f4 351 }
svenkugi 0:b6deec3905f4 352
svenkugi 0:b6deec3905f4 353 //dist_pid.setProcessValue(total_rev);
svenkugi 0:b6deec3905f4 354 //dutyout = dist_pid.compute();
svenkugi 0:b6deec3905f4 355
svenkugi 0:b6deec3905f4 356 //tmp.stop();
svenkugi 0:b6deec3905f4 357 //test_time = tmp.read();
svenkugi 0:b6deec3905f4 358 //tmp.reset();
svenkugi 0:b6deec3905f4 359 }
svenkugi 0:b6deec3905f4 360
svenkugi 0:b6deec3905f4 361 /*_______________________Testing and Tuning Function__________________________*/
svenkugi 0:b6deec3905f4 362
svenkugi 0:b6deec3905f4 363 /*Measures Angular Velocity using PhotoInterrupters by checking time taken to go
svenkugi 0:b6deec3905f4 364 from State 4 to State 4 in this case. Avoid sensor phasing as it measures one
svenkugi 0:b6deec3905f4 365 complete cycle */
svenkugi 0:b6deec3905f4 366
svenkugi 0:b6deec3905f4 367 void meas_velocity(){
svenkugi 0:b6deec3905f4 368
svenkugi 0:b6deec3905f4 369 intState = readRotorState();
svenkugi 0:b6deec3905f4 370 driveto = (intState-orState+direction*lead+6)%6;
svenkugi 0:b6deec3905f4 371 motorOut(driveto);
svenkugi 0:b6deec3905f4 372
svenkugi 0:b6deec3905f4 373 while (1) {
svenkugi 0:b6deec3905f4 374
svenkugi 0:b6deec3905f4 375 pc.printf("Rotations per second: %f \n\r", angular_vel);
svenkugi 0:b6deec3905f4 376
svenkugi 0:b6deec3905f4 377 }
svenkugi 0:b6deec3905f4 378 }
svenkugi 0:b6deec3905f4 379
svenkugi 0:b6deec3905f4 380 void PID_tuning(){
svenkugi 0:b6deec3905f4 381
svenkugi 0:b6deec3905f4 382 dutyout = 0.5;
svenkugi 0:b6deec3905f4 383
svenkugi 0:b6deec3905f4 384 intState = readRotorState();
svenkugi 0:b6deec3905f4 385 driveto = (intState-orState+lead+6)%6;
svenkugi 0:b6deec3905f4 386 motorOut(driveto);
svenkugi 0:b6deec3905f4 387
svenkugi 0:b6deec3905f4 388 while (1) {
svenkugi 0:b6deec3905f4 389
svenkugi 0:b6deec3905f4 390 // Testing Step Response by increasing D.C. from 0.5 to 0.7
svenkugi 0:b6deec3905f4 391 // Gradient is equal to Kc
svenkugi 0:b6deec3905f4 392
svenkugi 0:b6deec3905f4 393 if(count > 3000){
svenkugi 0:b6deec3905f4 394 dutyout = 0.7;
svenkugi 0:b6deec3905f4 395 }
svenkugi 0:b6deec3905f4 396
svenkugi 0:b6deec3905f4 397 pc.printf("Duty Cycle: %f ", dutyout);
svenkugi 0:b6deec3905f4 398 pc.printf("Rotations per second: %f ", angular_vel);
svenkugi 0:b6deec3905f4 399 pc.printf("Count: %d \n\r", count);
svenkugi 0:b6deec3905f4 400
svenkugi 0:b6deec3905f4 401 }
svenkugi 0:b6deec3905f4 402 }
svenkugi 0:b6deec3905f4 403
svenkugi 0:b6deec3905f4 404 /*__________________________Main Function_____________________________________*/
svenkugi 0:b6deec3905f4 405 int main(){
svenkugi 0:b6deec3905f4 406
svenkugi 0:b6deec3905f4 407 //Start of Program
svenkugi 0:b6deec3905f4 408 pc.printf("STARTING SKAFMO BRUSHLESS MOTOR PROJECT! \n\r");
svenkugi 0:b6deec3905f4 409 led1 = 1;
svenkugi 0:b6deec3905f4 410
svenkugi 0:b6deec3905f4 411 //Run the motor synchronisation: orState is subtracted from future rotor state inputs
svenkugi 0:b6deec3905f4 412 orState = motorHome();
svenkugi 0:b6deec3905f4 413 //pc.printf("Synchronization Complete: Rotor and Motor aligned with Offset: %x\n\r",orState);
svenkugi 0:b6deec3905f4 414
svenkugi 0:b6deec3905f4 415 //Interrupts (Optical Disk State Change): Drives to next state, Measures whole revolution count, Measures angular velocity over a whole revolution
svenkugi 0:b6deec3905f4 416 InterruptI1.rise(&changestate_isr);
svenkugi 0:b6deec3905f4 417 InterruptI1.fall(&changestate_isr);
svenkugi 0:b6deec3905f4 418 InterruptI2.rise(&changestate_isr);
svenkugi 0:b6deec3905f4 419 InterruptI2.fall(&changestate_isr);
svenkugi 0:b6deec3905f4 420 InterruptI3.rise(&changestate_isr);
svenkugi 0:b6deec3905f4 421 InterruptI3.fall(&changestate_isr);
svenkugi 0:b6deec3905f4 422
svenkugi 0:b6deec3905f4 423 //Interrupts (Incremental Encoder CHA Phase)
svenkugi 0:b6deec3905f4 424 InterruptCHA.rise(&pid_isr);
svenkugi 0:b6deec3905f4 425
svenkugi 0:b6deec3905f4 426 //Thread to wait for regex commands
svenkugi 0:b6deec3905f4 427 //thread.start(waitcommand_thread);
svenkugi 0:b6deec3905f4 428
svenkugi 0:b6deec3905f4 429 //position_control(10.0, 1, 2.0); // Second parameter is 1 or -1 (Backwards or Forwards Rotation)
svenkugi 0:b6deec3905f4 430
svenkugi 0:b6deec3905f4 431 }
svenkugi 0:b6deec3905f4 432
svenkugi 0:b6deec3905f4 433
svenkugi 0:b6deec3905f4 434
svenkugi 0:b6deec3905f4 435 /*__________________________________________TO DO_______________________________
svenkugi 0:b6deec3905f4 436
svenkugi 0:b6deec3905f4 437 Solve jitter problem when it is just at the edge of the motorhome state
svenkugi 0:b6deec3905f4 438 Acceleration profile
svenkugi 0:b6deec3905f4 439 Incremental Encorder
svenkugi 0:b6deec3905f4 440 Profiling to find the overhead of each thread or task
svenkugi 0:b6deec3905f4 441 Minimize drift error, reinitialize
svenkugi 0:b6deec3905f4 442 Function to check if stuck, and give software push
svenkugi 0:b6deec3905f4 443 Characterize Low Pass Filtering Characteristic of Motor (Draw Freq. Spectrum)
svenkugi 0:b6deec3905f4 444 Play melody as you spin with adjustable frequency and same duty cycle
svenkugi 0:b6deec3905f4 445
svenkugi 0:b6deec3905f4 446 */