Amber Tang / Mbed 2 deprecated JS_1motor_20170707_ok01

123

Dependencies:   PID Servo mbed

Fork of JS_1motor_20170707_ok by Amber Tang

main.cpp@2:868e7c902ba6, 2017-07-08 (annotated)

Committer:
Amber77
Date:
Sat Jul 08 09:37:51 2017 +0000
Revision:
2:868e7c902ba6
Parent:
1:bfdd210b730a 
7 to 8

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Amber77 0:39ee6f693e49 1 /* "PID.h"--- https://developer.mbed.org/users/aberk/code/PID/ */
Amber77 0:39ee6f693e49 2 /* "Servo.h"-- https://developer.mbed.org/users/andrewrussell/code/Servo/file/4c315bcd91b4/Servo.cpp */
Amber77 0:39ee6f693e49 3 /* JS using 3.3V */
Amber77 0:39ee6f693e49 4
Amber77 0:39ee6f693e49 5 #include "mbed.h"
Amber77 0:39ee6f693e49 6 #include "PID.h"
Amber77 0:39ee6f693e49 7 #include "Servo.h"
Amber77 0:39ee6f693e49 8 //****************************************************************************
Amber77 0:39ee6f693e49 9 int encoderClickCount = 0; // hold the signed value corresponding to the number of clicks left or right since last sample
Amber77 0:39ee6f693e49 10 int previousEncoderState = 0; // keep a record of the last actioned sample state of the Qb+Qa outputs for comparison on each interrupt
Amber77 0:39ee6f693e49 11 float x, y, press = 0;
Amber77 0:39ee6f693e49 12 float cx, cy = 0;
Amber77 0:39ee6f693e49 13 int px, py = 0;
Amber77 0:39ee6f693e49 14 int bia = 3;
Amber77 0:39ee6f693e49 15 float X_pos, Y_pos, CW_CCW1, CW_CCW2, stopRun = 0;
Amber77 0:39ee6f693e49 16 //****************************************************************************
Amber77 0:39ee6f693e49 17 DigitalOut StopRun(D15);
Amber77 0:39ee6f693e49 18 DigitalOut Brake(D14);
Amber77 0:39ee6f693e49 19 DigitalOut AR(D9);
Amber77 0:39ee6f693e49 20 DigitalOut X1_CW_CCW(PC_6);
Amber77 0:39ee6f693e49 21 DigitalOut X2_CW_CCW(D2);
Amber77 0:39ee6f693e49 22 DigitalOut Y1_CW_CCW(PC_8);
Amber77 0:39ee6f693e49 23 DigitalOut Y2_CW_CCW(PC_9);
Amber77 0:39ee6f693e49 24
Amber77 0:39ee6f693e49 25 Servo X_PWM(D6);
Amber77 2:868e7c902ba6 26 Servo Y_PWM(D8);
Amber77 0:39ee6f693e49 27
Amber77 0:39ee6f693e49 28 //DigitalIn phaseA(A0); // phase A of the quadrature encoder
Amber77 0:39ee6f693e49 29 //DigitalIn phaseB(A1); // phase B of the quadrature encoder
Amber77 0:39ee6f693e49 30 DigitalIn phaseA( PC_2 ); // phase a of the quadrature encoder
Amber77 0:39ee6f693e49 31 DigitalIn phaseB( PC_3 ); // phase b of the quadrature encoder
Amber77 0:39ee6f693e49 32 AnalogIn vx(A2);
Amber77 0:39ee6f693e49 33 AnalogIn vy(A3);
Amber77 0:39ee6f693e49 34 AnalogIn ps(A4);
Amber77 0:39ee6f693e49 35
Amber77 0:39ee6f693e49 36
Amber77 0:39ee6f693e49 37 void quadratureDecoder( void )
Amber77 0:39ee6f693e49 38 {
Amber77 0:39ee6f693e49 39 int currentEncoderState = (phaseB.read() << 1) + phaseA.read(); // create a two bit value out of phaseB and phaseA
Amber77 0:39ee6f693e49 40
Amber77 0:39ee6f693e49 41 if( currentEncoderState == previousEncoderState )
Amber77 0:39ee6f693e49 42 {
Amber77 0:39ee6f693e49 43 return;
Amber77 0:39ee6f693e49 44 }
Amber77 0:39ee6f693e49 45
Amber77 0:39ee6f693e49 46 switch( previousEncoderState )
Amber77 0:39ee6f693e49 47 {
Amber77 0:39ee6f693e49 48 case 0:
Amber77 0:39ee6f693e49 49 if( currentEncoderState == 2 )
Amber77 0:39ee6f693e49 50 {
Amber77 0:39ee6f693e49 51 encoderClickCount--;
Amber77 0:39ee6f693e49 52
Amber77 0:39ee6f693e49 53 }
Amber77 0:39ee6f693e49 54 else if( currentEncoderState == 1 )
Amber77 0:39ee6f693e49 55 {
Amber77 0:39ee6f693e49 56 encoderClickCount++;
Amber77 0:39ee6f693e49 57
Amber77 0:39ee6f693e49 58 }
Amber77 0:39ee6f693e49 59 break;
Amber77 0:39ee6f693e49 60
Amber77 0:39ee6f693e49 61 case 1:
Amber77 0:39ee6f693e49 62 if( currentEncoderState == 0 )
Amber77 0:39ee6f693e49 63 {
Amber77 0:39ee6f693e49 64 encoderClickCount--;
Amber77 0:39ee6f693e49 65
Amber77 0:39ee6f693e49 66 }
Amber77 0:39ee6f693e49 67 else if( currentEncoderState == 3 )
Amber77 0:39ee6f693e49 68 {
Amber77 0:39ee6f693e49 69 encoderClickCount++;
Amber77 0:39ee6f693e49 70
Amber77 0:39ee6f693e49 71 }
Amber77 0:39ee6f693e49 72 break;
Amber77 0:39ee6f693e49 73
Amber77 0:39ee6f693e49 74 case 2:
Amber77 0:39ee6f693e49 75 if( currentEncoderState == 3 )
Amber77 0:39ee6f693e49 76 {
Amber77 0:39ee6f693e49 77 encoderClickCount--;
Amber77 0:39ee6f693e49 78
Amber77 0:39ee6f693e49 79 }
Amber77 0:39ee6f693e49 80 else if( currentEncoderState == 0 )
Amber77 0:39ee6f693e49 81 {
Amber77 0:39ee6f693e49 82 encoderClickCount++;
Amber77 0:39ee6f693e49 83
Amber77 0:39ee6f693e49 84 }
Amber77 0:39ee6f693e49 85 break;
Amber77 0:39ee6f693e49 86
Amber77 0:39ee6f693e49 87 case 3:
Amber77 0:39ee6f693e49 88 if( currentEncoderState == 1 )
Amber77 0:39ee6f693e49 89 {
Amber77 0:39ee6f693e49 90 encoderClickCount--;
Amber77 0:39ee6f693e49 91
Amber77 0:39ee6f693e49 92 }
Amber77 0:39ee6f693e49 93 else if( currentEncoderState == 2 )
Amber77 0:39ee6f693e49 94 {
Amber77 0:39ee6f693e49 95 encoderClickCount++;
Amber77 0:39ee6f693e49 96
Amber77 0:39ee6f693e49 97 }
Amber77 0:39ee6f693e49 98 break;
Amber77 0:39ee6f693e49 99
Amber77 0:39ee6f693e49 100 default:
Amber77 0:39ee6f693e49 101 break;
Amber77 0:39ee6f693e49 102 }
Amber77 0:39ee6f693e49 103 previousEncoderState = currentEncoderState;
Amber77 0:39ee6f693e49 104 }
Amber77 0:39ee6f693e49 105
Amber77 0:39ee6f693e49 106 int getClicks( void )
Amber77 0:39ee6f693e49 107 {
Amber77 0:39ee6f693e49 108 int res = encoderClickCount; // this allows the knob to be rotated "while im not looking at it' and still return the
Amber77 0:39ee6f693e49 109
Amber77 0:39ee6f693e49 110 // encoderClickCount = 0; // actual number of clicks that have been rotated since last time I was checking it.
Amber77 0:39ee6f693e49 111
Amber77 0:39ee6f693e49 112 return res;
Amber77 0:39ee6f693e49 113 }
Amber77 0:39ee6f693e49 114
Amber77 0:39ee6f693e49 115 int main()
Amber77 0:39ee6f693e49 116 {
Amber77 0:39ee6f693e49 117
Amber77 0:39ee6f693e49 118 Serial pc( USBTX, USBRX );
Amber77 0:39ee6f693e49 119 pc.baud(115200);
Amber77 0:39ee6f693e49 120 Ticker sampleTicker; // create a timer to sample the encoder
Amber77 0:39ee6f693e49 121 int currentClicks;
Amber77 0:39ee6f693e49 122
Amber77 0:39ee6f693e49 123 phaseA.mode( PullUp ); // phaseA is set with a built in pull-up resistor
Amber77 0:39ee6f693e49 124 phaseB.mode( PullUp ); // phaseB is set with a built in pull-up resistor
Amber77 0:39ee6f693e49 125 sampleTicker.attach_us( &quadratureDecoder, 100 ); // make the quadrature decoder function check the knob once every 1000us = 1ms
Amber77 0:39ee6f693e49 126
Amber77 0:39ee6f693e49 127
Amber77 0:39ee6f693e49 128 //PWM.calibrate(0.02, 0.001, 0.002); //50HZ
Amber77 0:39ee6f693e49 129 //PWM.calibrate(0.02, 0.02*0.02, 1*0.02); //50HZ
Amber77 0:39ee6f693e49 130 //PWM.calibrate(0.0001, 0.08*0.0001, 1*0.0001); //10kHZ
Amber77 0:39ee6f693e49 131 //PWM.calibrate(0.00001, 0.08*0.00001, 1*0.00001); //100kHZ
Amber77 0:39ee6f693e49 132 //PWM.calibrate(0.00001, 0.2*0.00001, 0.8*0.00001); //100kHZ
Amber77 0:39ee6f693e49 133 X_PWM.calibrate(0.00005, 0.08*0.00005, 1*0.00005); //20kHZ
Amber77 0:39ee6f693e49 134 Y_PWM.calibrate(0.00005, 0.08*0.00005, 1*0.00005); //20kHZ
Amber77 0:39ee6f693e49 135 //PWM.calibrate(0.000025, 0*0.000025, 1*0.000025); //40kHZ
Amber77 0:39ee6f693e49 136 //PWM.calibrate(0.000005, 0*0.000025, 1*0.000025); //200kHZ
Amber77 0:39ee6f693e49 137 cx = vx.read();
Amber77 0:39ee6f693e49 138 cy = vy.read();
Amber77 0:39ee6f693e49 139 AR.write(1);
Amber77 0:39ee6f693e49 140
Amber77 0:39ee6f693e49 141 pc.printf( "Encoder test started\r\n" );
Amber77 0:39ee6f693e49 142 while( 1 )
Amber77 0:39ee6f693e49 143 {
Amber77 0:39ee6f693e49 144 currentClicks = getClicks();
Amber77 0:39ee6f693e49 145 Brake.write(1);
Amber77 0:39ee6f693e49 146 x = vx.read();
Amber77 0:39ee6f693e49 147 y = vy.read();
Amber77 0:39ee6f693e49 148 /* x-axis re-arrange */
Amber77 0:39ee6f693e49 149 if(x > cx)
Amber77 0:39ee6f693e49 150 {
Amber77 0:39ee6f693e49 151 px = (x-cx)/(1-cx)*100;
Amber77 0:39ee6f693e49 152 if(px < bia)
Amber77 0:39ee6f693e49 153 {
Amber77 0:39ee6f693e49 154 px = 0;
Amber77 0:39ee6f693e49 155 stopRun = 1;
Amber77 0:39ee6f693e49 156 CW_CCW1 = 1;
Amber77 0:39ee6f693e49 157 CW_CCW2 = 0;
Amber77 0:39ee6f693e49 158 X_pos = 0;
Amber77 0:39ee6f693e49 159 }
Amber77 0:39ee6f693e49 160 else
Amber77 0:39ee6f693e49 161 {
Amber77 0:39ee6f693e49 162 stopRun = 0;
Amber77 0:39ee6f693e49 163 CW_CCW1 = 1;
Amber77 0:39ee6f693e49 164 CW_CCW2 = 0;
Amber77 0:39ee6f693e49 165 X_pos = 0.5;
Amber77 0:39ee6f693e49 166 }
Amber77 0:39ee6f693e49 167 }
Amber77 0:39ee6f693e49 168 else if(x < cx)
Amber77 0:39ee6f693e49 169 {
Amber77 0:39ee6f693e49 170 px = (x-cx)/(cx-0)*100;
Amber77 0:39ee6f693e49 171 if(px > -bia)
Amber77 0:39ee6f693e49 172 {
Amber77 0:39ee6f693e49 173 px = 0;
Amber77 0:39ee6f693e49 174 stopRun = 1;
Amber77 0:39ee6f693e49 175 CW_CCW1 = 0;
Amber77 0:39ee6f693e49 176 CW_CCW2 = 1;
Amber77 0:39ee6f693e49 177 X_pos = 0;
Amber77 0:39ee6f693e49 178 }
Amber77 0:39ee6f693e49 179 else
Amber77 0:39ee6f693e49 180 {
Amber77 0:39ee6f693e49 181 stopRun = 0;
Amber77 0:39ee6f693e49 182 CW_CCW1 = 0;
Amber77 0:39ee6f693e49 183 CW_CCW2 = 1;
Amber77 0:39ee6f693e49 184 X_pos = 0.5;
Amber77 0:39ee6f693e49 185 }
Amber77 0:39ee6f693e49 186 }
Amber77 0:39ee6f693e49 187 else
Amber77 0:39ee6f693e49 188 px=cx;
Amber77 0:39ee6f693e49 189 /* y-axis re-arrange */
Amber77 0:39ee6f693e49 190 if(y > cy)
Amber77 0:39ee6f693e49 191 {
Amber77 0:39ee6f693e49 192 py = (y-cy)/(1-cy)*100;
Amber77 0:39ee6f693e49 193 if(py < bia)
Amber77 0:39ee6f693e49 194 {
Amber77 0:39ee6f693e49 195 py = 0;
Amber77 0:39ee6f693e49 196 stopRun = 1;
Amber77 0:39ee6f693e49 197 CW_CCW1 = 1;
Amber77 0:39ee6f693e49 198 CW_CCW2 = 0;
Amber77 0:39ee6f693e49 199 Y_pos = 0;
Amber77 0:39ee6f693e49 200 }
Amber77 0:39ee6f693e49 201 else
Amber77 0:39ee6f693e49 202 {
Amber77 0:39ee6f693e49 203
Amber77 0:39ee6f693e49 204 stopRun = 0;
Amber77 0:39ee6f693e49 205 CW_CCW1 = 1;
Amber77 0:39ee6f693e49 206 CW_CCW2 = 0;
Amber77 0:39ee6f693e49 207 Y_pos = 0.5;
Amber77 0:39ee6f693e49 208 }
Amber77 0:39ee6f693e49 209 }
Amber77 0:39ee6f693e49 210 else if(y < cy)
Amber77 0:39ee6f693e49 211 {
Amber77 0:39ee6f693e49 212 py = (y-cy)/(cy-0)*100;
Amber77 0:39ee6f693e49 213 if(py > -bia)
Amber77 0:39ee6f693e49 214 {
Amber77 0:39ee6f693e49 215 py = 0;
Amber77 0:39ee6f693e49 216 stopRun = 1;
Amber77 0:39ee6f693e49 217 CW_CCW1 = 0;
Amber77 0:39ee6f693e49 218 CW_CCW2 = 1;
Amber77 0:39ee6f693e49 219 Y_pos = 0;
Amber77 0:39ee6f693e49 220 }
Amber77 0:39ee6f693e49 221 else
Amber77 0:39ee6f693e49 222 {
Amber77 0:39ee6f693e49 223 stopRun = 0;
Amber77 0:39ee6f693e49 224 CW_CCW1 = 0;
Amber77 0:39ee6f693e49 225 CW_CCW2 = 1;
Amber77 0:39ee6f693e49 226 Y_pos = 0.5;
Amber77 0:39ee6f693e49 227 }
Amber77 0:39ee6f693e49 228 }
Amber77 0:39ee6f693e49 229 else
Amber77 0:39ee6f693e49 230 {
Amber77 0:39ee6f693e49 231 py=cy;
Amber77 0:39ee6f693e49 232 }
Amber77 0:39ee6f693e49 233 // while( 1 )
Amber77 0:39ee6f693e49 234 // {
Amber77 0:39ee6f693e49 235 // currentClicks = getClicks();
Amber77 0:39ee6f693e49 236 // Brake.write(1);
Amber77 0:39ee6f693e49 237 // x = vx.read();
Amber77 0:39ee6f693e49 238 // y = vy.read();
Amber77 0:39ee6f693e49 239 // /* x-axis re-arrange */
Amber77 0:39ee6f693e49 240 // if(x > cx)
Amber77 0:39ee6f693e49 241 // {
Amber77 0:39ee6f693e49 242 // px = (x-cx)/(1-cx)*100;
Amber77 0:39ee6f693e49 243 // if(px < bia)
Amber77 0:39ee6f693e49 244 // {
Amber77 0:39ee6f693e49 245 // px = 0;
Amber77 0:39ee6f693e49 246 // StopRun.write(1);
Amber77 0:39ee6f693e49 247 // X1_CW_CCW.write(1); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 248 // X2_CW_CCW.write(0); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 249 // Y1_CW_CCW.write(1); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 250 // Y2_CW_CCW.write(0); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 251 // X_pos = 0;
Amber77 0:39ee6f693e49 252 // Y_pos = 0;
Amber77 0:39ee6f693e49 253 // }
Amber77 0:39ee6f693e49 254 // else
Amber77 0:39ee6f693e49 255 // {
Amber77 0:39ee6f693e49 256 // StopRun.write(0);
Amber77 0:39ee6f693e49 257 //// CW_CCW.write(0); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 258 // X1_CW_CCW.write(1); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 259 // X2_CW_CCW.write(0); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 260 // Y1_CW_CCW.write(1); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 261 // Y2_CW_CCW.write(0); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 262 // X_pos = 0.5;
Amber77 0:39ee6f693e49 263 // Y_pos = 0;
Amber77 0:39ee6f693e49 264 // }
Amber77 0:39ee6f693e49 265 // }
Amber77 0:39ee6f693e49 266 // else if(x < cx)
Amber77 0:39ee6f693e49 267 // {
Amber77 0:39ee6f693e49 268 // px = (x-cx)/(cx-0)*100;
Amber77 0:39ee6f693e49 269 // if(px > -bia)
Amber77 0:39ee6f693e49 270 // {
Amber77 0:39ee6f693e49 271 // px = 0;
Amber77 0:39ee6f693e49 272 // StopRun.write(1);
Amber77 0:39ee6f693e49 273 //// CW_CCW.write(1); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 274 // X1_CW_CCW.write(0); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 275 // X2_CW_CCW.write(1); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 276 // Y1_CW_CCW.write(0); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 277 // Y2_CW_CCW.write(1); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 278 // X_pos = 0;
Amber77 0:39ee6f693e49 279 // Y_pos = 0;
Amber77 0:39ee6f693e49 280 // }
Amber77 0:39ee6f693e49 281 // else
Amber77 0:39ee6f693e49 282 // {
Amber77 0:39ee6f693e49 283 // StopRun.write(0);
Amber77 0:39ee6f693e49 284 //// CW_CCW.write(1); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 285 // X1_CW_CCW.write(0); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 286 // X2_CW_CCW.write(1); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 287 // Y1_CW_CCW.write(0); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 288 // Y2_CW_CCW.write(1); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 289 // X_pos = 0.5;
Amber77 0:39ee6f693e49 290 // Y_pos = 0;
Amber77 0:39ee6f693e49 291 // }
Amber77 0:39ee6f693e49 292 // }
Amber77 0:39ee6f693e49 293 // else
Amber77 0:39ee6f693e49 294 // px=cx;
Amber77 0:39ee6f693e49 295 // /* y-axis re-arrange */
Amber77 0:39ee6f693e49 296 // if(y > cy)
Amber77 0:39ee6f693e49 297 // {
Amber77 0:39ee6f693e49 298 // py = (y-cy)/(1-cy)*100;
Amber77 0:39ee6f693e49 299 // if(py < bia)
Amber77 0:39ee6f693e49 300 // {
Amber77 0:39ee6f693e49 301 // py = 0;
Amber77 0:39ee6f693e49 302 // StopRun.write(1);
Amber77 0:39ee6f693e49 303 // X1_CW_CCW.write(1); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 304 // X2_CW_CCW.write(0); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 305 // Y1_CW_CCW.write(1); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 306 // Y2_CW_CCW.write(0); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 307 // X_pos = 0;
Amber77 0:39ee6f693e49 308 // Y_pos = 0;
Amber77 0:39ee6f693e49 309 // }
Amber77 0:39ee6f693e49 310 // else
Amber77 0:39ee6f693e49 311 // {
Amber77 0:39ee6f693e49 312 //
Amber77 0:39ee6f693e49 313 // StopRun.write(0);
Amber77 0:39ee6f693e49 314 // X1_CW_CCW.write(1); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 315 // X2_CW_CCW.write(0); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 316 // Y1_CW_CCW.write(1); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 317 // Y2_CW_CCW.write(0); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 318 // X_pos = 0;
Amber77 0:39ee6f693e49 319 // Y_pos = 0.5;
Amber77 0:39ee6f693e49 320 // }
Amber77 0:39ee6f693e49 321 // }
Amber77 0:39ee6f693e49 322 // else if(y < cy)
Amber77 0:39ee6f693e49 323 // {
Amber77 0:39ee6f693e49 324 // py = (y-cy)/(cy-0)*100;
Amber77 0:39ee6f693e49 325 // if(py > -bia)
Amber77 0:39ee6f693e49 326 // {
Amber77 0:39ee6f693e49 327 // py = 0;
Amber77 0:39ee6f693e49 328 // StopRun.write(1);
Amber77 0:39ee6f693e49 329 // X1_CW_CCW.write(0); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 330 // X2_CW_CCW.write(1); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 331 // Y1_CW_CCW.write(0); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 332 // Y2_CW_CCW.write(1); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 333 // X_pos = 0;
Amber77 0:39ee6f693e49 334 // Y_pos = 0;
Amber77 0:39ee6f693e49 335 // }
Amber77 0:39ee6f693e49 336 // else
Amber77 0:39ee6f693e49 337 // {
Amber77 0:39ee6f693e49 338 // StopRun.write(0);
Amber77 0:39ee6f693e49 339 // X1_CW_CCW.write(0); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 340 // X2_CW_CCW.write(1); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 341 // Y1_CW_CCW.write(0); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 342 // Y2_CW_CCW.write(1); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 343 // X_pos = 0;
Amber77 0:39ee6f693e49 344 // Y_pos = 0.5;
Amber77 0:39ee6f693e49 345 // }
Amber77 0:39ee6f693e49 346 // }
Amber77 0:39ee6f693e49 347 // else
Amber77 0:39ee6f693e49 348 // {
Amber77 0:39ee6f693e49 349 // py=cy;
Amber77 0:39ee6f693e49 350 // }
Amber77 0:39ee6f693e49 351 /*if(x > cx)
Amber77 0:39ee6f693e49 352 {
Amber77 0:39ee6f693e49 353 px = (x-cx)/(1-cx)*100;
Amber77 0:39ee6f693e49 354 if(px < bia)
Amber77 0:39ee6f693e49 355 {
Amber77 0:39ee6f693e49 356 px = 0;
Amber77 0:39ee6f693e49 357 StopRun.write(1);
Amber77 0:39ee6f693e49 358 CW_CCW.write(0); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 359 pos = 0;
Amber77 0:39ee6f693e49 360 }
Amber77 0:39ee6f693e49 361 else
Amber77 0:39ee6f693e49 362 {
Amber77 0:39ee6f693e49 363 StopRun.write(0);
Amber77 0:39ee6f693e49 364 CW_CCW.write(0); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 365 pos=px*0.01;
Amber77 0:39ee6f693e49 366 }
Amber77 0:39ee6f693e49 367 }
Amber77 0:39ee6f693e49 368 else if(x < cx)
Amber77 0:39ee6f693e49 369 {
Amber77 0:39ee6f693e49 370 px = (x-cx)/(cx-0)*100;
Amber77 0:39ee6f693e49 371 if(px > -bia)
Amber77 0:39ee6f693e49 372 {
Amber77 0:39ee6f693e49 373 px = 0;
Amber77 0:39ee6f693e49 374 StopRun.write(1);
Amber77 0:39ee6f693e49 375 CW_CCW.write(1); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 376 pos = 0;
Amber77 0:39ee6f693e49 377 }
Amber77 0:39ee6f693e49 378 else
Amber77 0:39ee6f693e49 379 {
Amber77 0:39ee6f693e49 380 StopRun.write(0);
Amber77 0:39ee6f693e49 381 CW_CCW.write(1); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 382 pos=px*(-0.01);
Amber77 0:39ee6f693e49 383 }
Amber77 0:39ee6f693e49 384 }
Amber77 0:39ee6f693e49 385 else
Amber77 0:39ee6f693e49 386 px=cx;
Amber77 0:39ee6f693e49 387 /* y-axis re-arrange */
Amber77 0:39ee6f693e49 388 /*if(y > cy)
Amber77 0:39ee6f693e49 389 {
Amber77 0:39ee6f693e49 390 py = (y-cy)/(1-cy)*100;
Amber77 0:39ee6f693e49 391 if(py < bia)
Amber77 0:39ee6f693e49 392 py = 0;
Amber77 0:39ee6f693e49 393 }
Amber77 0:39ee6f693e49 394 else if(y < cy)
Amber77 0:39ee6f693e49 395 {
Amber77 0:39ee6f693e49 396 py = (y-cy)/(cy-0)*100;
Amber77 0:39ee6f693e49 397 if(py > -bia)
Amber77 0:39ee6f693e49 398 py = 0;
Amber77 0:39ee6f693e49 399 }
Amber77 0:39ee6f693e49 400 else
Amber77 0:39ee6f693e49 401 {
Amber77 0:39ee6f693e49 402 py=cy;
Amber77 0:39ee6f693e49 403 }
Amber77 0:39ee6f693e49 404 */
Amber77 0:39ee6f693e49 405 StopRun.write(stopRun);
Amber77 0:39ee6f693e49 406 X1_CW_CCW.write(CW_CCW1); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 407 X2_CW_CCW.write(CW_CCW2); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 408 Y1_CW_CCW.write(CW_CCW1); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 409 Y2_CW_CCW.write(CW_CCW2); // clockwise:0 counterclockwise:1
Amber77 0:39ee6f693e49 410 X_PWM.write(X_pos);
Amber77 0:39ee6f693e49 411 Y_PWM.write(Y_pos);
Amber77 0:39ee6f693e49 412
Amber77 0:39ee6f693e49 413
Amber77 0:39ee6f693e49 414 press = ps.read();
Amber77 0:39ee6f693e49 415 if(press < 0.008)
Amber77 0:39ee6f693e49 416 printf("Pressed: %d, %d\n",px,py);
Amber77 0:39ee6f693e49 417 else
Amber77 0:39ee6f693e49 418 printf(" : %d, %d\n",px,py);
Amber77 0:39ee6f693e49 419
Amber77 0:39ee6f693e49 420 wait_ms(0.1);
Amber77 0:39ee6f693e49 421
Amber77 0:39ee6f693e49 422 if( currentClicks != 0 )
Amber77 0:39ee6f693e49 423 {
Amber77 0:39ee6f693e49 424 //pc.printf( "%f, %d, %f || click count = %d\r\n", x, px, pos, currentClicks/*, previousEncoderState */);
Amber77 0:39ee6f693e49 425 //pc.printf("%f, %d, %f\n",x,px,pos);
Amber77 0:39ee6f693e49 426 pc.printf( "%f || %f || click count = %d\r\n", X_pos, Y_pos, currentClicks/*, previousEncoderState */);
Amber77 0:39ee6f693e49 427 }
Amber77 0:39ee6f693e49 428 wait_ms( 20 );
Amber77 0:39ee6f693e49 429 }
Amber77 0:39ee6f693e49 430 }