Diagonaal berekenen lukt, meerdere outputs nog niet

Dependencies:   mbed FastPWM HIDScope MODSERIAL QEI

Committer:
sjoerdbarts
Date:
Thu Oct 20 12:07:30 2016 +0000
Revision:
6:ee243782bf51
Parent:
5:a9da7bc89b24
Initial working code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TimDouma 0:46a1642a10c8 1 #include "mbed.h"
sjoerdbarts 6:ee243782bf51 2 #include "FastPWM.h"
sjoerdbarts 6:ee243782bf51 3 #include "HIDScope.h"
sjoerdbarts 6:ee243782bf51 4 #include "QEI.h"
sjoerdbarts 6:ee243782bf51 5 #include "MODSERIAL.h"
TimDouma 0:46a1642a10c8 6 #define SERIAL_BAUD 115200
sjoerdbarts 6:ee243782bf51 7
sjoerdbarts 6:ee243782bf51 8 MODSERIAL pc(USBTX,USBRX);
sjoerdbarts 6:ee243782bf51 9 // Set button pins
sjoerdbarts 6:ee243782bf51 10 InterruptIn button1(PTB9);
sjoerdbarts 6:ee243782bf51 11 InterruptIn button2(PTA1);
TimDouma 0:46a1642a10c8 12
sjoerdbarts 6:ee243782bf51 13 // Set AnalogIn potmeters
sjoerdbarts 6:ee243782bf51 14 AnalogIn pot1(A3);
sjoerdbarts 6:ee243782bf51 15 AnalogIn pot2(A4);
TimDouma 0:46a1642a10c8 16
sjoerdbarts 6:ee243782bf51 17 // Set motor Pinouts
sjoerdbarts 6:ee243782bf51 18 DigitalOut motor1_dir(D4);
sjoerdbarts 6:ee243782bf51 19 FastPWM motor1_pwm(D5);
sjoerdbarts 6:ee243782bf51 20 DigitalOut motor2_dir(D7);
sjoerdbarts 6:ee243782bf51 21 FastPWM motor2_pwm(D6);
s1600907 3:ac6f86c0db6e 22
sjoerdbarts 6:ee243782bf51 23 // Set LED pins
sjoerdbarts 6:ee243782bf51 24 DigitalOut led(LED_RED);
s1600907 5:a9da7bc89b24 25
sjoerdbarts 6:ee243782bf51 26 // Set HID scope
sjoerdbarts 6:ee243782bf51 27 HIDScope scope(2);
s1600907 5:a9da7bc89b24 28
sjoerdbarts 6:ee243782bf51 29 // Set Encoders for motors
sjoerdbarts 6:ee243782bf51 30 QEI Motor1_ENC_CW(D11,D10,NC,32);
sjoerdbarts 6:ee243782bf51 31 QEI Motor1_ENC_CCW(D10,D11,NC,32);
sjoerdbarts 6:ee243782bf51 32 QEI Motor2_ENC_CW(D12,D13,NC,32);
sjoerdbarts 6:ee243782bf51 33 QEI Motor2_ENC_CCW(D13,D12,NC,32);
sjoerdbarts 6:ee243782bf51 34
sjoerdbarts 6:ee243782bf51 35 // Constants
sjoerdbarts 6:ee243782bf51 36 volatile double pwm = 0.0;
sjoerdbarts 6:ee243782bf51 37 volatile double pwm_new = 0;
sjoerdbarts 6:ee243782bf51 38 volatile double timer = 0.0;
sjoerdbarts 6:ee243782bf51 39 volatile bool button1_value = false;
sjoerdbarts 6:ee243782bf51 40 volatile bool button2_value = false;
s1600907 4:bd1db680e5a1 41
sjoerdbarts 6:ee243782bf51 42 // Ticker
sjoerdbarts 6:ee243782bf51 43 Ticker SinTicker;
sjoerdbarts 6:ee243782bf51 44 Ticker PotControlTicker;
sjoerdbarts 6:ee243782bf51 45 Ticker SendZerosTicker;
sjoerdbarts 6:ee243782bf51 46
sjoerdbarts 6:ee243782bf51 47 void SwitchLed()
sjoerdbarts 6:ee243782bf51 48 {
sjoerdbarts 6:ee243782bf51 49 led = not led;
sjoerdbarts 6:ee243782bf51 50 }
sjoerdbarts 6:ee243782bf51 51
sjoerdbarts 6:ee243782bf51 52 void button1_switch()
sjoerdbarts 6:ee243782bf51 53 {
sjoerdbarts 6:ee243782bf51 54 button1_value = not button1_value;
sjoerdbarts 6:ee243782bf51 55 }
sjoerdbarts 6:ee243782bf51 56
sjoerdbarts 6:ee243782bf51 57 void button2_switch()
sjoerdbarts 6:ee243782bf51 58 {
sjoerdbarts 6:ee243782bf51 59 button2_value = not button2_value;
s1600907 4:bd1db680e5a1 60 }
TimDouma 0:46a1642a10c8 61
sjoerdbarts 6:ee243782bf51 62 void SendZeros()
sjoerdbarts 6:ee243782bf51 63 {
sjoerdbarts 6:ee243782bf51 64 int zero=0;
sjoerdbarts 6:ee243782bf51 65 scope.set(0,zero);
sjoerdbarts 6:ee243782bf51 66 scope.set(1,zero);
sjoerdbarts 6:ee243782bf51 67 scope.send();
TimDouma 0:46a1642a10c8 68 }
TimDouma 0:46a1642a10c8 69
sjoerdbarts 6:ee243782bf51 70 void SetMotor(int motor_number, double MotorValue)
sjoerdbarts 6:ee243782bf51 71 {
sjoerdbarts 6:ee243782bf51 72 // Given -1<=motorValue<=1, this sets the PWM and direction
sjoerdbarts 6:ee243782bf51 73 // bits for motor 1. Positive value makes motor rotating
sjoerdbarts 6:ee243782bf51 74 // clockwise. motorValues outside range are truncated to
sjoerdbarts 6:ee243782bf51 75 // within range
sjoerdbarts 6:ee243782bf51 76 if (motor_number == 1)
sjoerdbarts 6:ee243782bf51 77 {
sjoerdbarts 6:ee243782bf51 78 if (MotorValue >=0)
sjoerdbarts 6:ee243782bf51 79 {
sjoerdbarts 6:ee243782bf51 80 motor1_dir=1;
sjoerdbarts 6:ee243782bf51 81 }
sjoerdbarts 6:ee243782bf51 82 else
sjoerdbarts 6:ee243782bf51 83 {
sjoerdbarts 6:ee243782bf51 84 motor1_dir=0;
sjoerdbarts 6:ee243782bf51 85 }
sjoerdbarts 6:ee243782bf51 86 if (fabs(MotorValue)>1){
sjoerdbarts 6:ee243782bf51 87 motor1_pwm.write(1);
sjoerdbarts 6:ee243782bf51 88 }
sjoerdbarts 6:ee243782bf51 89 else
sjoerdbarts 6:ee243782bf51 90 {
sjoerdbarts 6:ee243782bf51 91 motor1_pwm.write(abs(MotorValue));
sjoerdbarts 6:ee243782bf51 92 }
s1600907 5:a9da7bc89b24 93 }
sjoerdbarts 6:ee243782bf51 94 else
sjoerdbarts 6:ee243782bf51 95 {
sjoerdbarts 6:ee243782bf51 96 if (MotorValue >=0)
sjoerdbarts 6:ee243782bf51 97 {
sjoerdbarts 6:ee243782bf51 98 motor1_dir=1;
sjoerdbarts 6:ee243782bf51 99 }
sjoerdbarts 6:ee243782bf51 100 else
sjoerdbarts 6:ee243782bf51 101 {
sjoerdbarts 6:ee243782bf51 102 motor1_dir=0;
sjoerdbarts 6:ee243782bf51 103 }
sjoerdbarts 6:ee243782bf51 104 if (fabs(MotorValue)>1){
sjoerdbarts 6:ee243782bf51 105 motor1_pwm.write(1);
sjoerdbarts 6:ee243782bf51 106 }
sjoerdbarts 6:ee243782bf51 107 else
sjoerdbarts 6:ee243782bf51 108 {
sjoerdbarts 6:ee243782bf51 109 motor1_pwm.write(abs(MotorValue));
sjoerdbarts 6:ee243782bf51 110 }
s1600907 4:bd1db680e5a1 111 }
sjoerdbarts 6:ee243782bf51 112
sjoerdbarts 6:ee243782bf51 113 }
sjoerdbarts 6:ee243782bf51 114
sjoerdbarts 6:ee243782bf51 115 double GetPosition(int motor_number)
sjoerdbarts 6:ee243782bf51 116 {
sjoerdbarts 6:ee243782bf51 117 const int COUNTS_PER_REV = 8400;
sjoerdbarts 6:ee243782bf51 118 const float DEGREES_PER_PULSE = 8400 / 360;
sjoerdbarts 6:ee243782bf51 119 if (motor_number == 1)
sjoerdbarts 6:ee243782bf51 120 {
sjoerdbarts 6:ee243782bf51 121 int countsCW = Motor1_ENC_CW.getPulses();
sjoerdbarts 6:ee243782bf51 122 int countsCCW= Motor1_ENC_CCW.getPulses();
sjoerdbarts 6:ee243782bf51 123 int net_counts = countsCW-countsCCW;
sjoerdbarts 6:ee243782bf51 124 double Position=(net_counts*360.0f)/COUNTS_PER_REV;
sjoerdbarts 6:ee243782bf51 125 return Position;
s1600907 4:bd1db680e5a1 126 }
sjoerdbarts 6:ee243782bf51 127 else
sjoerdbarts 6:ee243782bf51 128 {
sjoerdbarts 6:ee243782bf51 129 int countsCW = Motor2_ENC_CW.getPulses();
sjoerdbarts 6:ee243782bf51 130 int countsCCW= Motor2_ENC_CCW.getPulses();
sjoerdbarts 6:ee243782bf51 131 int net_counts = countsCW-countsCCW;
sjoerdbarts 6:ee243782bf51 132 double Position=(net_counts*360.0f)/COUNTS_PER_REV;
sjoerdbarts 6:ee243782bf51 133 return Position;
s1600907 4:bd1db680e5a1 134 }
s1600907 4:bd1db680e5a1 135 }
s1600907 5:a9da7bc89b24 136
sjoerdbarts 6:ee243782bf51 137 void MultiSin_motor1()
sjoerdbarts 6:ee243782bf51 138 {
sjoerdbarts 6:ee243782bf51 139 double pwm = pwm_new;
sjoerdbarts 6:ee243782bf51 140 double f1 = 0.2f;
sjoerdbarts 6:ee243782bf51 141 double f2 = 1.0f;
sjoerdbarts 6:ee243782bf51 142 double f3 = 5.0f;
sjoerdbarts 6:ee243782bf51 143 double f4 = 20.0f;
sjoerdbarts 6:ee243782bf51 144 double f5 = 100.0f;
sjoerdbarts 6:ee243782bf51 145 const double pi = 3.141592653589793238462;
sjoerdbarts 6:ee243782bf51 146 // read encoder
sjoerdbarts 6:ee243782bf51 147 double motor1_position = GetPosition(1);
sjoerdbarts 6:ee243782bf51 148 // set HIDSCOPE values, position, pwm*volt
sjoerdbarts 6:ee243782bf51 149 scope.set(0, motor1_position);
sjoerdbarts 6:ee243782bf51 150 scope.set(1, pwm);
sjoerdbarts 6:ee243782bf51 151 // sent HIDScope values
sjoerdbarts 6:ee243782bf51 152 scope.send();
sjoerdbarts 6:ee243782bf51 153 // calculate the new multisin
sjoerdbarts 6:ee243782bf51 154 double multisin = sin(2*pi*f1*timer)+sin(2*pi*f2*timer)+sin(2*pi*f3*timer)+sin(2*pi*f4*timer)+sin(2*pi*f5*timer);
sjoerdbarts 6:ee243782bf51 155 // devide by the amount of sin waves
sjoerdbarts 6:ee243782bf51 156 pwm_new=multisin / 5;
sjoerdbarts 6:ee243782bf51 157 // write the motors
sjoerdbarts 6:ee243782bf51 158 SetMotor(1, multisin);
sjoerdbarts 6:ee243782bf51 159 // Increase the timer
sjoerdbarts 6:ee243782bf51 160 timer = timer + 0.001;
sjoerdbarts 6:ee243782bf51 161 if (timer >= (20.0-0.001))
sjoerdbarts 6:ee243782bf51 162 {
sjoerdbarts 6:ee243782bf51 163 SinTicker.detach();
sjoerdbarts 6:ee243782bf51 164 timer = 0;
sjoerdbarts 6:ee243782bf51 165 }
sjoerdbarts 6:ee243782bf51 166 }
sjoerdbarts 6:ee243782bf51 167
sjoerdbarts 6:ee243782bf51 168 void MultiSin_motor2()
sjoerdbarts 6:ee243782bf51 169 {
sjoerdbarts 6:ee243782bf51 170 double pwm = pwm_new;
sjoerdbarts 6:ee243782bf51 171 double f1 = 0.2f;
sjoerdbarts 6:ee243782bf51 172 double f2 = 1.0f;
sjoerdbarts 6:ee243782bf51 173 double f3 = 5.0f;
sjoerdbarts 6:ee243782bf51 174 double f4 = 20.0f;
sjoerdbarts 6:ee243782bf51 175 double f5 = 100.0f;
sjoerdbarts 6:ee243782bf51 176 const double pi = 3.141592653589793238462;
sjoerdbarts 6:ee243782bf51 177 // read encoder
sjoerdbarts 6:ee243782bf51 178 double motor2_position = GetPosition(2);
sjoerdbarts 6:ee243782bf51 179 // set HIDSCOPE values, position, pwm*volt
sjoerdbarts 6:ee243782bf51 180 scope.set(0, motor2_position);
sjoerdbarts 6:ee243782bf51 181 scope.set(1, pwm);
sjoerdbarts 6:ee243782bf51 182 // sent HIDScope values
sjoerdbarts 6:ee243782bf51 183 scope.send();
sjoerdbarts 6:ee243782bf51 184 // calculate the new multisin
sjoerdbarts 6:ee243782bf51 185 double multisin = sin(2*pi*f1*timer)+sin(2*pi*f2*timer)+sin(2*pi*f3*timer)+sin(2*pi*f4*timer)+sin(2*pi*f5*timer);
sjoerdbarts 6:ee243782bf51 186 // devide by the amount of sin waves
sjoerdbarts 6:ee243782bf51 187 pwm_new=multisin / 5;
sjoerdbarts 6:ee243782bf51 188 // write the motor
sjoerdbarts 6:ee243782bf51 189 SetMotor(2, multisin);
sjoerdbarts 6:ee243782bf51 190 // Increase the timer
sjoerdbarts 6:ee243782bf51 191 timer = timer + 0.001;
sjoerdbarts 6:ee243782bf51 192 if (timer >= (20.0-0.001))
sjoerdbarts 6:ee243782bf51 193 {
sjoerdbarts 6:ee243782bf51 194 SinTicker.detach();
sjoerdbarts 6:ee243782bf51 195 timer = 0;
sjoerdbarts 6:ee243782bf51 196 }
sjoerdbarts 6:ee243782bf51 197 }
sjoerdbarts 6:ee243782bf51 198
sjoerdbarts 6:ee243782bf51 199 void PotControl()
sjoerdbarts 6:ee243782bf51 200 {
sjoerdbarts 6:ee243782bf51 201 double Motor1_Value = (pot1.read() - 0.5)/2.0;
sjoerdbarts 6:ee243782bf51 202 double Motor2_Value = (pot2.read() - 0.5)/2.0;
sjoerdbarts 6:ee243782bf51 203 //pc.printf("\r\n Motor value 1: %f \r\n",Motor1_Value);
sjoerdbarts 6:ee243782bf51 204 //pc.printf("\r\n Motor value 2: %f \r\n",Motor2_Value);
sjoerdbarts 6:ee243782bf51 205 scope.set(0, Motor1_Value);
sjoerdbarts 6:ee243782bf51 206 scope.set(1, Motor2_Value);
sjoerdbarts 6:ee243782bf51 207 scope.send();
sjoerdbarts 6:ee243782bf51 208 // Write the motors
sjoerdbarts 6:ee243782bf51 209 SetMotor(1, Motor1_Value);
sjoerdbarts 6:ee243782bf51 210 SetMotor(2, Motor2_Value);
sjoerdbarts 6:ee243782bf51 211 }
sjoerdbarts 6:ee243782bf51 212
sjoerdbarts 6:ee243782bf51 213 void SinControl1()
sjoerdbarts 6:ee243782bf51 214 {
sjoerdbarts 6:ee243782bf51 215 SinTicker.attach(&MultiSin_motor1, 0.001);
sjoerdbarts 6:ee243782bf51 216 }
sjoerdbarts 6:ee243782bf51 217
sjoerdbarts 6:ee243782bf51 218 void SinControl2()
sjoerdbarts 6:ee243782bf51 219 {
sjoerdbarts 6:ee243782bf51 220 SinTicker.attach(&MultiSin_motor2, 0.001);
sjoerdbarts 6:ee243782bf51 221 }
sjoerdbarts 6:ee243782bf51 222
sjoerdbarts 6:ee243782bf51 223 void HIDScope_Send()
sjoerdbarts 6:ee243782bf51 224 {
sjoerdbarts 6:ee243782bf51 225 scope.send();
sjoerdbarts 6:ee243782bf51 226 }
sjoerdbarts 6:ee243782bf51 227
sjoerdbarts 6:ee243782bf51 228
sjoerdbarts 6:ee243782bf51 229
s1600907 3:ac6f86c0db6e 230 int main() {
s1600907 3:ac6f86c0db6e 231 pc.baud(SERIAL_BAUD);
sjoerdbarts 6:ee243782bf51 232 pc.printf("\r\n ***THERMONUCLEAR WARFARE HAS COMMENCED*** \r\n");
sjoerdbarts 6:ee243782bf51 233
sjoerdbarts 6:ee243782bf51 234 // Set LED
sjoerdbarts 6:ee243782bf51 235 led=0;
sjoerdbarts 6:ee243782bf51 236 Ticker LedTicker;
sjoerdbarts 6:ee243782bf51 237 LedTicker.attach(SwitchLed,0.5f);
sjoerdbarts 6:ee243782bf51 238
sjoerdbarts 6:ee243782bf51 239 // Interrupt for buttons
sjoerdbarts 6:ee243782bf51 240 button1.fall(button1_switch);
sjoerdbarts 6:ee243782bf51 241 button2.fall(button2_switch);
sjoerdbarts 6:ee243782bf51 242
sjoerdbarts 6:ee243782bf51 243 pc.printf("\r\n ***************** \r\n");
sjoerdbarts 6:ee243782bf51 244 pc.printf("\r\n Press button 1 to start positioning the arms \r\n");
sjoerdbarts 6:ee243782bf51 245 pc.printf("\r\n Make sure both potentiometers are positioned halfway! \r\n");
sjoerdbarts 6:ee243782bf51 246 pc.printf("\r\n ***************** \r\n");
sjoerdbarts 6:ee243782bf51 247 while (button1_value == false)
sjoerdbarts 6:ee243782bf51 248 {
sjoerdbarts 6:ee243782bf51 249 // just wait
sjoerdbarts 6:ee243782bf51 250 }
sjoerdbarts 6:ee243782bf51 251 pc.printf("\r\n ***************** \r\n");
sjoerdbarts 6:ee243782bf51 252 pc.printf("\r\n Use potentiometers to control the motor arms \r\n");
sjoerdbarts 6:ee243782bf51 253 pc.printf("\r\n Pot 1 for motor 1 \r\n");
sjoerdbarts 6:ee243782bf51 254 pc.printf("\r\n Pot 2 for motor 2 \r\n");
sjoerdbarts 6:ee243782bf51 255 pc.printf("\r\n ***************** \r\n");
sjoerdbarts 6:ee243782bf51 256 pc.printf("\r\n Press button 2 to start the rest of the program \r\n");
sjoerdbarts 6:ee243782bf51 257 pc.printf("\r\n ***************** \r\n");
sjoerdbarts 6:ee243782bf51 258 // Start potmeter control
sjoerdbarts 6:ee243782bf51 259 PotControlTicker.attach(&PotControl,0.01f);
TimDouma 0:46a1642a10c8 260
sjoerdbarts 6:ee243782bf51 261 while (button2_value == false)
sjoerdbarts 6:ee243782bf51 262 {
sjoerdbarts 6:ee243782bf51 263 // just wait
sjoerdbarts 6:ee243782bf51 264 }
sjoerdbarts 6:ee243782bf51 265 PotControlTicker.detach();
sjoerdbarts 6:ee243782bf51 266 SendZerosTicker.attach(&SendZeros,0.001);
sjoerdbarts 6:ee243782bf51 267
sjoerdbarts 6:ee243782bf51 268 // Program startup
sjoerdbarts 6:ee243782bf51 269 pc.printf("\r\n Starting multisin on motor 1 in: \r\n");
sjoerdbarts 6:ee243782bf51 270 pc.printf("\r\n 10 \r\n");
sjoerdbarts 6:ee243782bf51 271 wait(5);
sjoerdbarts 6:ee243782bf51 272 pc.printf("\r\n 5 \r\n");
sjoerdbarts 6:ee243782bf51 273 wait(1);
sjoerdbarts 6:ee243782bf51 274 pc.printf("\r\n 4 \r\n");
sjoerdbarts 6:ee243782bf51 275 wait(1);
sjoerdbarts 6:ee243782bf51 276 pc.printf("\r\n 3 \r\n");
sjoerdbarts 6:ee243782bf51 277 wait(1);
sjoerdbarts 6:ee243782bf51 278 pc.printf("\r\n 2 \r\n");
sjoerdbarts 6:ee243782bf51 279 wait(1);
sjoerdbarts 6:ee243782bf51 280 pc.printf("\r\n 1 \r\n");
sjoerdbarts 6:ee243782bf51 281 wait(1);
sjoerdbarts 6:ee243782bf51 282 pc.printf("\r\n 0 \r\n");
sjoerdbarts 6:ee243782bf51 283 pc.printf("\r\n Wait for the signal to complete \r\n");
sjoerdbarts 6:ee243782bf51 284 pc.printf("\r\n Press button 2 too continue afterwards \r\n");
sjoerdbarts 6:ee243782bf51 285 SendZerosTicker.detach();
sjoerdbarts 6:ee243782bf51 286 SinControl1();
sjoerdbarts 6:ee243782bf51 287
sjoerdbarts 6:ee243782bf51 288
sjoerdbarts 6:ee243782bf51 289 while (button2_value == true)
sjoerdbarts 6:ee243782bf51 290 {
sjoerdbarts 6:ee243782bf51 291 // just wait
s1600907 5:a9da7bc89b24 292 }
s1600907 5:a9da7bc89b24 293
TimDouma 2:bf1466ac4e6f 294
sjoerdbarts 6:ee243782bf51 295 SendZerosTicker.attach(&SendZeros,0.001);
sjoerdbarts 6:ee243782bf51 296 pc.printf("\r\n Starting multisin on motor 2 in: \r\n");
sjoerdbarts 6:ee243782bf51 297 pc.printf("\r\n 10 \r\n");
sjoerdbarts 6:ee243782bf51 298 wait(5);
sjoerdbarts 6:ee243782bf51 299 pc.printf("\r\n 5 \r\n");
sjoerdbarts 6:ee243782bf51 300 wait(1);
sjoerdbarts 6:ee243782bf51 301 pc.printf("\r\n 4 \r\n");
sjoerdbarts 6:ee243782bf51 302 wait(1);
sjoerdbarts 6:ee243782bf51 303 pc.printf("\r\n 3 \r\n");
sjoerdbarts 6:ee243782bf51 304 wait(1);
sjoerdbarts 6:ee243782bf51 305 pc.printf("\r\n 2 \r\n");
sjoerdbarts 6:ee243782bf51 306 wait(1);
sjoerdbarts 6:ee243782bf51 307 pc.printf("\r\n 1 \r\n");
sjoerdbarts 6:ee243782bf51 308 wait(1);
sjoerdbarts 6:ee243782bf51 309 pc.printf("\r\n 0 \r\n");
sjoerdbarts 6:ee243782bf51 310 SendZerosTicker.detach();
sjoerdbarts 6:ee243782bf51 311 SinControl2();
sjoerdbarts 6:ee243782bf51 312 wait(23);
sjoerdbarts 6:ee243782bf51 313 pc.printf("\r\n Program Finished, press reset to restart \r\n");
sjoerdbarts 6:ee243782bf51 314 while (true) {}
TimDouma 0:46a1642a10c8 315 }