Radu Radoveneanu / Mbed OS moonlitemockup

Telescope motorised focuser based on the moonlite protocol, indilib's indi_monlite_focuser compliant. WIP

main.cpp@6:c74fa5932b21, 2018-08-01 (annotated)

Committer:
rakware
Date:
Wed Aug 01 17:35:14 2018 +0000
Revision:
6:c74fa5932b21
Parent:
5:8965c2919afb
Child:
7:30dcdf917330
test build

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rakware 2:c75920ffd4b1 1 #include "mbed.h" //uses https://github.com/ARMmbed/mbed-os/
rakware 2:c75920ffd4b1 2
rakware 3:20b077586e4e 3 DigitalOut led(LED1); //board led, just for fun
rakware 3:20b077586e4e 4
rakware 2:c75920ffd4b1 5 Serial pc(USBTX, USBRX); // tx, rx pc control over usb
rakware 0:c990fafbe0fc 6
rakware 3:20b077586e4e 7 uint16_t gn = 0; //position to move to
rakware 3:20b077586e4e 8 uint16_t gp = 0; //current position
rakware 3:20b077586e4e 9 uint16_t temperature = 255; //hardcoded for now //TODO
rakware 3:20b077586e4e 10
rakware 3:20b077586e4e 11 bool is_moving, move = false;
rakware 2:c75920ffd4b1 12
rakware 3:20b077586e4e 13 uint16_t step_delay = 256;
rakware 5:8965c2919afb 14
rakware 5:8965c2919afb 15 //minimum motor speed that will get doubled 5 times to result maximum motor speed
rakware 5:8965c2919afb 16 //eg: for a value of 100 there will be 5 speeds as follows 100*1,100*2,100*4,100*8,100*16
rakware 5:8965c2919afb 17 //eg: for a value of 100 there will be 5 speeds as follows 100,200,400,800,1600
rakware 5:8965c2919afb 18 uint8_t min_motor_pps = 75;
rakware 4:2235d6d01ca5 19
rakware 3:20b077586e4e 20
rakware 3:20b077586e4e 21 char ml_in[256];
rakware 3:20b077586e4e 22 uint8_t idx = 0;
rakware 3:20b077586e4e 23
rakware 3:20b077586e4e 24 Thread led_thread, motor_thread;
rakware 0:c990fafbe0fc 25
rakware 2:c75920ffd4b1 26 void led_blink()
rakware 2:c75920ffd4b1 27 {
rakware 2:c75920ffd4b1 28 while (true) {
rakware 2:c75920ffd4b1 29 led = !led;
rakware 2:c75920ffd4b1 30 wait(1);
rakware 2:c75920ffd4b1 31 }
rakware 2:c75920ffd4b1 32 }
rakware 2:c75920ffd4b1 33
rakware 0:c990fafbe0fc 34
rakware 5:8965c2919afb 35 //motor control with MP6500 https://www.pololu.com/product/2968
rakware 5:8965c2919afb 36 //aproximate formula for motor current control pwm.pulsewidth% = (motor_rated_current_in_amps-2.2)/-2.079, so for our 330ma motor we use pw = (0.330-2.2)/-2.079
rakware 5:8965c2919afb 37 // https://www.pololu.com/product/2968#lightbox-picture0J8398;main-pictures , 1 kHz or greater, I2 set to low
rakware 4:2235d6d01ca5 38 //D2,D3,D4,D5,D6,D7,D8,D9 - DIR, STEP, SLP, I2, I1, MS2, MS1, EN
rakware 4:2235d6d01ca5 39
rakware 4:2235d6d01ca5 40 DigitalOut dir(D2);
rakware 4:2235d6d01ca5 41 DigitalOut step(D3);
rakware 4:2235d6d01ca5 42 DigitalOut slp(D4); //pull high to enable operation
rakware 5:8965c2919afb 43 DigitalOut i2(D5); //set low to enable pwm power control
rakware 4:2235d6d01ca5 44 PwmOut i1(D6); //MP6500 power limit control
rakware 4:2235d6d01ca5 45 DigitalOut ms2(D7);
rakware 4:2235d6d01ca5 46 DigitalOut ms1(D8); //set low for FULL or high for HALF, only use MS1 to be moonlite compliant ?
rakware 4:2235d6d01ca5 47 DigitalOut en(D9); //pulled low by the motor driver, default enables operation
rakware 4:2235d6d01ca5 48
rakware 5:8965c2919afb 49 float motor_max_amps = 0.33f;
rakware 4:2235d6d01ca5 50
rakware 5:8965c2919afb 51 float pw = (motor_max_amps-2.2f)/-2.079f;
rakware 0:c990fafbe0fc 52
rakware 4:2235d6d01ca5 53 void mp6500() //TODO enable/disable motor
rakware 4:2235d6d01ca5 54 {
rakware 4:2235d6d01ca5 55 dir = 0;
rakware 4:2235d6d01ca5 56 step = 0;
rakware 4:2235d6d01ca5 57 slp = 1;
rakware 4:2235d6d01ca5 58 i2 = 0;
rakware 4:2235d6d01ca5 59 i1.period(0.001f); //1kHz period
rakware 5:8965c2919afb 60 i1.write(pw);
rakware 4:2235d6d01ca5 61 ms2 = 0;
rakware 5:8965c2919afb 62 ms1 = 1;
rakware 4:2235d6d01ca5 63 en = 0;
rakware 4:2235d6d01ca5 64 }
rakware 4:2235d6d01ca5 65
rakware 4:2235d6d01ca5 66
rakware 3:20b077586e4e 67 void motor()
rakware 3:20b077586e4e 68 {
rakware 2:c75920ffd4b1 69
rakware 3:20b077586e4e 70 while (true) {
rakware 3:20b077586e4e 71
rakware 3:20b077586e4e 72 if(move) {
rakware 5:8965c2919afb 73
rakware 5:8965c2919afb 74 i1.write(pw); //set defined current motor while moving
rakware 5:8965c2919afb 75
rakware 3:20b077586e4e 76 is_moving = true;
rakware 3:20b077586e4e 77
rakware 3:20b077586e4e 78 if(gp > gn) {
rakware 3:20b077586e4e 79
rakware 4:2235d6d01ca5 80 dir = 0;
rakware 5:8965c2919afb 81 wait_us(1);
rakware 5:8965c2919afb 82 //__disable_irq(); // Disable Interrupts because serial irq makes motor move funky with indi_moonlite_focuser polling
rakware 4:2235d6d01ca5 83 //__enable_irq(); // Enable Interrupts
rakware 5:8965c2919afb 84 //led = !led;
rakware 4:2235d6d01ca5 85 step = !step;
rakware 5:8965c2919afb 86 wait_us(1); //mp6500
rakware 5:8965c2919afb 87 //led = !led;
rakware 4:2235d6d01ca5 88 step = !step;
rakware 5:8965c2919afb 89 wait_us(1);
rakware 5:8965c2919afb 90 //https://os.mbed.com/docs/v5.9/mbed-os-api-doxy/mbed__wait__api_8h_source.html use Thread::Wait()?
rakware 5:8965c2919afb 91 wait((step_delay/1.024)/min_motor_pps/250);
rakware 3:20b077586e4e 92 gp--;
rakware 3:20b077586e4e 93
rakware 3:20b077586e4e 94 } else if (gp < gn) {
rakware 0:c990fafbe0fc 95
rakware 4:2235d6d01ca5 96 dir = 1;
rakware 5:8965c2919afb 97 wait_us(1);
rakware 5:8965c2919afb 98 //led = !led;
rakware 4:2235d6d01ca5 99 step = !step;
rakware 5:8965c2919afb 100 wait_us(1);
rakware 5:8965c2919afb 101 //led = !led;
rakware 4:2235d6d01ca5 102 step = !step;
rakware 5:8965c2919afb 103 wait_us(1);
rakware 5:8965c2919afb 104 wait((step_delay/1.024)/min_motor_pps/250);
rakware 3:20b077586e4e 105 gp++;
rakware 3:20b077586e4e 106
rakware 3:20b077586e4e 107 } else {
rakware 3:20b077586e4e 108
rakware 4:2235d6d01ca5 109 step = dir = 0;
rakware 5:8965c2919afb 110 wait_us(1);
rakware 5:8965c2919afb 111
rakware 6:c74fa5932b21 112 i1.write(1.0f); //set minimum controller power while holding, keeps motor cool and still provides a nice holding torque, ~120ma
rakware 4:2235d6d01ca5 113
rakware 3:20b077586e4e 114 is_moving = false;
rakware 3:20b077586e4e 115 move = false;
rakware 0:c990fafbe0fc 116
rakware 3:20b077586e4e 117 }
rakware 3:20b077586e4e 118
rakware 3:20b077586e4e 119 }
rakware 3:20b077586e4e 120
rakware 3:20b077586e4e 121 }
rakware 3:20b077586e4e 122
rakware 3:20b077586e4e 123 }
rakware 3:20b077586e4e 124
rakware 3:20b077586e4e 125
rakware 3:20b077586e4e 126
rakware 3:20b077586e4e 127 //http://indilib.org/media/kunena/attachments/1/HighResSteppermotor107.pdf
rakware 2:c75920ffd4b1 128 void read_serial()
rakware 0:c990fafbe0fc 129 {
rakware 0:c990fafbe0fc 130
rakware 0:c990fafbe0fc 131 while(pc.readable()) {
rakware 0:c990fafbe0fc 132 char c = pc.getc();
rakware 0:c990fafbe0fc 133
rakware 0:c990fafbe0fc 134 switch (c) {
rakware 0:c990fafbe0fc 135 case '#':
rakware 0:c990fafbe0fc 136
rakware 0:c990fafbe0fc 137 switch (ml_in[0]) {
rakware 0:c990fafbe0fc 138 case 'C': //N/A Initiate a temperature conversion; the conversion process takes a maximum of 750 milliseconds. The value returned by the :GT# command will not be valid until the conversion process completes.
rakware 2:c75920ffd4b1 139 break; //TODO
rakware 0:c990fafbe0fc 140 case 'F':
rakware 0:c990fafbe0fc 141 switch (ml_in[1]) {
rakware 0:c990fafbe0fc 142 case 'G': //N/A Go to the new position as set by the ":SNYYYY#" command.
rakware 4:2235d6d01ca5 143 move = true; //runs motor();
rakware 0:c990fafbe0fc 144 break;
rakware 0:c990fafbe0fc 145 case 'Q': //N/A Immediately stop any focus motor movement.
rakware 3:20b077586e4e 146 move = false;
rakware 0:c990fafbe0fc 147 break;
rakware 0:c990fafbe0fc 148 default:
rakware 0:c990fafbe0fc 149 break;
rakware 0:c990fafbe0fc 150 }
rakware 0:c990fafbe0fc 151 break;
rakware 0:c990fafbe0fc 152
rakware 0:c990fafbe0fc 153 case 'G':
rakware 0:c990fafbe0fc 154 switch (ml_in[1]) {
rakware 0:c990fafbe0fc 155 case 'B': // Get the backlight value
rakware 0:c990fafbe0fc 156 pc.printf("00#");
rakware 0:c990fafbe0fc 157 break;
rakware 0:c990fafbe0fc 158 case 'C': //XX# Returns the temperature coefficient where XX is a two-digit signed (2’s complement) hex number.
rakware 0:c990fafbe0fc 159 pc.printf("02#");
rakware 0:c990fafbe0fc 160 break;
rakware 0:c990fafbe0fc 161 case 'D': //XX# Returns the current stepping delay where XX is a two-digit unsigned hex number. Valid values 02, 04, 08, 10, 20 -> stepping delay 250, 125, 63, 32, 16 pps
rakware 4:2235d6d01ca5 162 pc.printf("%02X#", 512 / step_delay); //TODO? we sleep for step_delay*step_delay_multiplier microseconds between steps
rakware 0:c990fafbe0fc 163 break;
rakware 0:c990fafbe0fc 164 case 'H': //00# OR FF# Returns "FF#" if the focus motor is half-stepped otherwise return "00#"
rakware 4:2235d6d01ca5 165 //pc.printf("00#"); //TODO
rakware 4:2235d6d01ca5 166 pc.printf("%02X#", ms1 * 255);
rakware 0:c990fafbe0fc 167 break;
rakware 0:c990fafbe0fc 168 case 'I': //00# OR 01# Returns "00#" if the focus motor is not moving, otherwise return "01#"
rakware 3:20b077586e4e 169 pc.printf("%02X#", is_moving);
rakware 0:c990fafbe0fc 170 break;
rakware 2:c75920ffd4b1 171 case 'N': //YYYY# Returns the new position previously set by a ":SNYYYY#" command where YYYY is a four-digit unsigned hex number
rakware 2:c75920ffd4b1 172 pc.printf("%04X#", gn); //TODO use this to move the motor
rakware 0:c990fafbe0fc 173 break;
rakware 0:c990fafbe0fc 174 case 'P': //YYYY# Returns the current position where YYYY is a four-digit unsigned hex number.
rakware 2:c75920ffd4b1 175 pc.printf("%04X#", gp); //TODO update gp with actual motor data
rakware 0:c990fafbe0fc 176 break;
rakware 0:c990fafbe0fc 177 case 'T': //YYYY# Returns the current temperature where YYYY is a four-digit signed (2’s complement) hex number.
rakware 2:c75920ffd4b1 178 pc.printf("%04X#", temperature * 2); //indi_moonlite_focuser returns the temp/2, dunno why, dont care
rakware 4:2235d6d01ca5 179 //pc.printf("0019#"); //should be 25c but is 12.5c in indi_moonlite control panel
rakware 0:c990fafbe0fc 180 break;
rakware 0:c990fafbe0fc 181 case 'V': //DD# Get the version of the firmware as a two-digit decimal number where the first digit is the major version number, and the second digit is the minor version number.
rakware 0:c990fafbe0fc 182 pc.printf("01#");
rakware 0:c990fafbe0fc 183 break;
rakware 0:c990fafbe0fc 184 default:
rakware 0:c990fafbe0fc 185 break;
rakware 0:c990fafbe0fc 186 }
rakware 0:c990fafbe0fc 187 break;
rakware 0:c990fafbe0fc 188
rakware 0:c990fafbe0fc 189 case 'S':
rakware 0:c990fafbe0fc 190 switch(ml_in[1]) {
rakware 0:c990fafbe0fc 191 case 'C': //N/A Set the new temperature coefficient where XX is a two-digit, signed (2’s complement) hex number.
rakware 2:c75920ffd4b1 192 break; //TODO
rakware 0:c990fafbe0fc 193 case 'D': //N/A Set the new stepping delay where XX is a two-digit, unsigned hex number. Valid values to send are 02, 04, 08, 10 and 20, which correspond to a stepping delay of 250, 125, 63, 32 and 16 steps per second respectively.
rakware 3:20b077586e4e 194 step_delay = 512 / strtol(ml_in + 2, NULL, 16);
rakware 0:c990fafbe0fc 195 break;
rakware 0:c990fafbe0fc 196
rakware 0:c990fafbe0fc 197 case 'F': //N/A Set full-step mode.
rakware 4:2235d6d01ca5 198 ms1 = 0;
rakware 0:c990fafbe0fc 199 break;
rakware 0:c990fafbe0fc 200
rakware 0:c990fafbe0fc 201 case 'H': //N/A Set half-step mode.
rakware 4:2235d6d01ca5 202 ms1 = 1;
rakware 0:c990fafbe0fc 203 break;
rakware 0:c990fafbe0fc 204
rakware 0:c990fafbe0fc 205 case 'N': //N/A Set the new position where YYYY is a four-digit unsigned hex number.
rakware 3:20b077586e4e 206 gn = strtol(ml_in + 2, NULL, 16); //read ml_in, discard 2 chars, convert to long, set gn
rakware 0:c990fafbe0fc 207 break;
rakware 0:c990fafbe0fc 208
rakware 0:c990fafbe0fc 209 case 'P': //Set the current position where YYYY is a four-digit unsigned hex number.
rakware 2:c75920ffd4b1 210 gp = strtol(ml_in + 2, NULL, 16);
rakware 0:c990fafbe0fc 211 break;
rakware 0:c990fafbe0fc 212
rakware 0:c990fafbe0fc 213 default:
rakware 0:c990fafbe0fc 214 break;
rakware 0:c990fafbe0fc 215 }
rakware 0:c990fafbe0fc 216 break;
rakware 0:c990fafbe0fc 217
rakware 0:c990fafbe0fc 218 case '+': //N/A Activate temperature compensation focusing.
rakware 0:c990fafbe0fc 219 break; //TODO
rakware 0:c990fafbe0fc 220 case '-': //N/A Disable temperature compensation focusing.
rakware 0:c990fafbe0fc 221 break; //TODO
rakware 0:c990fafbe0fc 222
rakware 0:c990fafbe0fc 223 case 'P':
rakware 0:c990fafbe0fc 224 break;
rakware 0:c990fafbe0fc 225
rakware 0:c990fafbe0fc 226 // case ':POXX#': //N/A Temperature calibration offset, XX is a two-digit signed hex number, in half degree increments. Example 1: :PO02# offset of +1°C Example 2: :POFB# offset of -2.5°
rakware 0:c990fafbe0fc 227 // break; //TODO
rakware 0:c990fafbe0fc 228
rakware 0:c990fafbe0fc 229 default:
rakware 0:c990fafbe0fc 230 break;
rakware 0:c990fafbe0fc 231 }
rakware 0:c990fafbe0fc 232 break;
rakware 0:c990fafbe0fc 233
rakware 0:c990fafbe0fc 234 case ':':
rakware 0:c990fafbe0fc 235 idx=0;
rakware 0:c990fafbe0fc 236 memset(ml_in, 0, 8);
rakware 0:c990fafbe0fc 237 break;
rakware 0:c990fafbe0fc 238
rakware 0:c990fafbe0fc 239 default:
rakware 0:c990fafbe0fc 240 ml_in[idx++] = c;
rakware 0:c990fafbe0fc 241 idx %= 8;
rakware 0:c990fafbe0fc 242 break;
rakware 0:c990fafbe0fc 243 }
rakware 0:c990fafbe0fc 244
rakware 0:c990fafbe0fc 245
rakware 0:c990fafbe0fc 246
rakware 0:c990fafbe0fc 247 }
rakware 4:2235d6d01ca5 248
rakware 0:c990fafbe0fc 249 }
rakware 0:c990fafbe0fc 250
rakware 0:c990fafbe0fc 251
rakware 0:c990fafbe0fc 252 int main()
rakware 0:c990fafbe0fc 253 {
rakware 4:2235d6d01ca5 254 mp6500();
rakware 0:c990fafbe0fc 255
rakware 3:20b077586e4e 256 //led_thread.start(led_blink);
rakware 4:2235d6d01ca5 257 pc.attach(read_serial, Serial::RxIrq);
rakware 4:2235d6d01ca5 258
rakware 3:20b077586e4e 259 motor_thread.start(motor);
rakware 4:2235d6d01ca5 260
rakware 4:2235d6d01ca5 261 while(true) {
rakware 4:2235d6d01ca5 262 Thread::wait(100);
rakware 4:2235d6d01ca5 263 //read_serial();
rakware 4:2235d6d01ca5 264 }
rakware 0:c990fafbe0fc 265
rakware 0:c990fafbe0fc 266 }