Biorobotics_group_2 / Mbed 2 deprecated System_Identification

Dynamics ident of the system

Dependencies:   FastPWM HIDScope MODSERIAL QEI mbed

Fork of Berekenen_motorhoek by Biorobotics_group_2

main.cpp@7:167b6dfdefee, 2016-10-28 (annotated)

Committer:
sjoerdbarts
Date:
Fri Oct 28 10:10:32 2016 +0000
Revision:
7:167b6dfdefee
Parent:
6:ee243782bf51 
Updated 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 7:167b6dfdefee 27 HIDScope scope(3);
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 7:167b6dfdefee 38 volatile double timer = 0.00000000;
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 7:167b6dfdefee 67 scope.set(2,zero);
sjoerdbarts 6:ee243782bf51 68 scope.send();
TimDouma 0:46a1642a10c8 69 }
TimDouma 0:46a1642a10c8 70
sjoerdbarts 6:ee243782bf51 71 void SetMotor(int motor_number, double MotorValue)
sjoerdbarts 6:ee243782bf51 72 {
sjoerdbarts 6:ee243782bf51 73 // Given -1<=motorValue<=1, this sets the PWM and direction
sjoerdbarts 6:ee243782bf51 74 // bits for motor 1. Positive value makes motor rotating
sjoerdbarts 6:ee243782bf51 75 // clockwise. motorValues outside range are truncated to
sjoerdbarts 6:ee243782bf51 76 // within range
sjoerdbarts 6:ee243782bf51 77 if (motor_number == 1)
sjoerdbarts 6:ee243782bf51 78 {
sjoerdbarts 6:ee243782bf51 79 if (MotorValue >=0)
sjoerdbarts 6:ee243782bf51 80 {
sjoerdbarts 6:ee243782bf51 81 motor1_dir=1;
sjoerdbarts 6:ee243782bf51 82 }
sjoerdbarts 6:ee243782bf51 83 else
sjoerdbarts 6:ee243782bf51 84 {
sjoerdbarts 6:ee243782bf51 85 motor1_dir=0;
sjoerdbarts 6:ee243782bf51 86 }
sjoerdbarts 6:ee243782bf51 87 if (fabs(MotorValue)>1){
sjoerdbarts 6:ee243782bf51 88 motor1_pwm.write(1);
sjoerdbarts 6:ee243782bf51 89 }
sjoerdbarts 6:ee243782bf51 90 else
sjoerdbarts 6:ee243782bf51 91 {
sjoerdbarts 6:ee243782bf51 92 motor1_pwm.write(abs(MotorValue));
sjoerdbarts 6:ee243782bf51 93 }
s1600907 5:a9da7bc89b24 94 }
sjoerdbarts 6:ee243782bf51 95 else
sjoerdbarts 6:ee243782bf51 96 {
sjoerdbarts 6:ee243782bf51 97 if (MotorValue >=0)
sjoerdbarts 6:ee243782bf51 98 {
sjoerdbarts 7:167b6dfdefee 99 motor2_dir=1;
sjoerdbarts 6:ee243782bf51 100 }
sjoerdbarts 6:ee243782bf51 101 else
sjoerdbarts 6:ee243782bf51 102 {
sjoerdbarts 7:167b6dfdefee 103 motor2_dir=0;
sjoerdbarts 6:ee243782bf51 104 }
sjoerdbarts 6:ee243782bf51 105 if (fabs(MotorValue)>1){
sjoerdbarts 7:167b6dfdefee 106 motor2_pwm.write(1);
sjoerdbarts 6:ee243782bf51 107 }
sjoerdbarts 6:ee243782bf51 108 else
sjoerdbarts 6:ee243782bf51 109 {
sjoerdbarts 7:167b6dfdefee 110 motor2_pwm.write(abs(MotorValue));
sjoerdbarts 6:ee243782bf51 111 }
s1600907 4:bd1db680e5a1 112 }
sjoerdbarts 6:ee243782bf51 113
sjoerdbarts 6:ee243782bf51 114 }
sjoerdbarts 6:ee243782bf51 115
sjoerdbarts 6:ee243782bf51 116 double GetPosition(int motor_number)
sjoerdbarts 6:ee243782bf51 117 {
sjoerdbarts 6:ee243782bf51 118 const int COUNTS_PER_REV = 8400;
sjoerdbarts 6:ee243782bf51 119 const float DEGREES_PER_PULSE = 8400 / 360;
sjoerdbarts 6:ee243782bf51 120 if (motor_number == 1)
sjoerdbarts 6:ee243782bf51 121 {
sjoerdbarts 6:ee243782bf51 122 int countsCW = Motor1_ENC_CW.getPulses();
sjoerdbarts 6:ee243782bf51 123 int countsCCW= Motor1_ENC_CCW.getPulses();
sjoerdbarts 6:ee243782bf51 124 int net_counts = countsCW-countsCCW;
sjoerdbarts 6:ee243782bf51 125 double Position=(net_counts*360.0f)/COUNTS_PER_REV;
sjoerdbarts 6:ee243782bf51 126 return Position;
s1600907 4:bd1db680e5a1 127 }
sjoerdbarts 6:ee243782bf51 128 else
sjoerdbarts 6:ee243782bf51 129 {
sjoerdbarts 6:ee243782bf51 130 int countsCW = Motor2_ENC_CW.getPulses();
sjoerdbarts 6:ee243782bf51 131 int countsCCW= Motor2_ENC_CCW.getPulses();
sjoerdbarts 6:ee243782bf51 132 int net_counts = countsCW-countsCCW;
sjoerdbarts 6:ee243782bf51 133 double Position=(net_counts*360.0f)/COUNTS_PER_REV;
sjoerdbarts 6:ee243782bf51 134 return Position;
s1600907 4:bd1db680e5a1 135 }
s1600907 4:bd1db680e5a1 136 }
s1600907 5:a9da7bc89b24 137
sjoerdbarts 6:ee243782bf51 138 void MultiSin_motor1()
sjoerdbarts 6:ee243782bf51 139 {
sjoerdbarts 6:ee243782bf51 140 double f1 = 0.2f;
sjoerdbarts 7:167b6dfdefee 141 double f2 = 0.4f;
sjoerdbarts 7:167b6dfdefee 142 double f3 = 0.8f;
sjoerdbarts 7:167b6dfdefee 143 double f4 = 1.6f;
sjoerdbarts 7:167b6dfdefee 144 double f5 = 3.2f;
sjoerdbarts 7:167b6dfdefee 145 double f6 = 6.3f;
sjoerdbarts 7:167b6dfdefee 146 double f7 = 12.6f;
sjoerdbarts 7:167b6dfdefee 147 double f8 = 25.0f;
sjoerdbarts 7:167b6dfdefee 148 double f9 = 50.0f;
sjoerdbarts 7:167b6dfdefee 149 double f10 = 100.0f;
sjoerdbarts 6:ee243782bf51 150 const double pi = 3.141592653589793238462;
sjoerdbarts 6:ee243782bf51 151 // read encoder
sjoerdbarts 6:ee243782bf51 152 double motor1_position = GetPosition(1);
sjoerdbarts 6:ee243782bf51 153 // set HIDSCOPE values, position, pwm*volt
sjoerdbarts 6:ee243782bf51 154 scope.set(0, motor1_position);
sjoerdbarts 7:167b6dfdefee 155 scope.set(1, pwm_new);
sjoerdbarts 7:167b6dfdefee 156 scope.set(2, timer);
sjoerdbarts 6:ee243782bf51 157 scope.send();
sjoerdbarts 7:167b6dfdefee 158 // sent HIDScope values
sjoerdbarts 7:167b6dfdefee 159
sjoerdbarts 6:ee243782bf51 160 // calculate the new multisin
sjoerdbarts 7:167b6dfdefee 161 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)+sin(2*pi*f6*timer)+sin(2*pi*f7*timer)+sin(2*pi*f8*timer)+sin(2*pi*f9*timer)+sin(2*pi*f10*timer);
sjoerdbarts 6:ee243782bf51 162 // devide by the amount of sin waves
sjoerdbarts 7:167b6dfdefee 163 pwm_new=multisin / 10.0f;
sjoerdbarts 6:ee243782bf51 164 // write the motors
sjoerdbarts 7:167b6dfdefee 165 SetMotor(1, pwm_new);
sjoerdbarts 6:ee243782bf51 166 // Increase the timer
sjoerdbarts 7:167b6dfdefee 167 timer = timer + 0.001f;
sjoerdbarts 7:167b6dfdefee 168 if (timer >= (10.00000f))
sjoerdbarts 6:ee243782bf51 169 {
sjoerdbarts 7:167b6dfdefee 170 SetMotor(1, 0);
sjoerdbarts 6:ee243782bf51 171 SinTicker.detach();
sjoerdbarts 6:ee243782bf51 172 timer = 0;
sjoerdbarts 6:ee243782bf51 173 }
sjoerdbarts 6:ee243782bf51 174 }
sjoerdbarts 6:ee243782bf51 175
sjoerdbarts 6:ee243782bf51 176 void MultiSin_motor2()
sjoerdbarts 6:ee243782bf51 177 {
sjoerdbarts 6:ee243782bf51 178 double f1 = 0.2f;
sjoerdbarts 7:167b6dfdefee 179 double f2 = 0.4f;
sjoerdbarts 7:167b6dfdefee 180 double f3 = 0.8f;
sjoerdbarts 7:167b6dfdefee 181 double f4 = 1.6f;
sjoerdbarts 7:167b6dfdefee 182 double f5 = 3.2f;
sjoerdbarts 7:167b6dfdefee 183 double f6 = 6.3f;
sjoerdbarts 7:167b6dfdefee 184 double f7 = 12.6f;
sjoerdbarts 7:167b6dfdefee 185 double f8 = 25.0f;
sjoerdbarts 7:167b6dfdefee 186 double f9 = 50.0f;
sjoerdbarts 7:167b6dfdefee 187 double f10 = 100.0f;
sjoerdbarts 6:ee243782bf51 188 const double pi = 3.141592653589793238462;
sjoerdbarts 6:ee243782bf51 189 // read encoder
sjoerdbarts 6:ee243782bf51 190 double motor2_position = GetPosition(2);
sjoerdbarts 6:ee243782bf51 191 // set HIDSCOPE values, position, pwm*volt
sjoerdbarts 6:ee243782bf51 192 scope.set(0, motor2_position);
sjoerdbarts 7:167b6dfdefee 193 scope.set(1, pwm_new);
sjoerdbarts 7:167b6dfdefee 194 scope.set(2, timer);
sjoerdbarts 6:ee243782bf51 195 scope.send();
sjoerdbarts 7:167b6dfdefee 196
sjoerdbarts 6:ee243782bf51 197 // calculate the new multisin
sjoerdbarts 7:167b6dfdefee 198 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)+sin(2*pi*f6*timer)+sin(2*pi*f7*timer)+sin(2*pi*f8*timer)+sin(2*pi*f9*timer)+sin(2*pi*f10*timer);
sjoerdbarts 6:ee243782bf51 199 // devide by the amount of sin waves
sjoerdbarts 7:167b6dfdefee 200 pwm_new=multisin / 10.0f;
sjoerdbarts 7:167b6dfdefee 201 // write the motors
sjoerdbarts 7:167b6dfdefee 202 SetMotor(2, pwm_new);
sjoerdbarts 6:ee243782bf51 203 // Increase the timer
sjoerdbarts 7:167b6dfdefee 204 timer = timer + 0.001f;
sjoerdbarts 7:167b6dfdefee 205 if (timer >= (10.0000f))
sjoerdbarts 6:ee243782bf51 206 {
sjoerdbarts 7:167b6dfdefee 207 SetMotor(2, 0);
sjoerdbarts 6:ee243782bf51 208 SinTicker.detach();
sjoerdbarts 6:ee243782bf51 209 timer = 0;
sjoerdbarts 6:ee243782bf51 210 }
sjoerdbarts 6:ee243782bf51 211 }
sjoerdbarts 6:ee243782bf51 212
sjoerdbarts 6:ee243782bf51 213 void PotControl()
sjoerdbarts 6:ee243782bf51 214 {
sjoerdbarts 6:ee243782bf51 215 double Motor1_Value = (pot1.read() - 0.5)/2.0;
sjoerdbarts 6:ee243782bf51 216 double Motor2_Value = (pot2.read() - 0.5)/2.0;
sjoerdbarts 6:ee243782bf51 217 //pc.printf("\r\n Motor value 1: %f \r\n",Motor1_Value);
sjoerdbarts 6:ee243782bf51 218 //pc.printf("\r\n Motor value 2: %f \r\n",Motor2_Value);
sjoerdbarts 6:ee243782bf51 219 scope.set(0, Motor1_Value);
sjoerdbarts 6:ee243782bf51 220 scope.set(1, Motor2_Value);
sjoerdbarts 6:ee243782bf51 221 scope.send();
sjoerdbarts 6:ee243782bf51 222 // Write the motors
sjoerdbarts 6:ee243782bf51 223 SetMotor(1, Motor1_Value);
sjoerdbarts 6:ee243782bf51 224 SetMotor(2, Motor2_Value);
sjoerdbarts 6:ee243782bf51 225 }
sjoerdbarts 6:ee243782bf51 226
sjoerdbarts 6:ee243782bf51 227 void SinControl1()
sjoerdbarts 6:ee243782bf51 228 {
sjoerdbarts 6:ee243782bf51 229 SinTicker.attach(&MultiSin_motor1, 0.001);
sjoerdbarts 6:ee243782bf51 230 }
sjoerdbarts 6:ee243782bf51 231
sjoerdbarts 6:ee243782bf51 232 void SinControl2()
sjoerdbarts 6:ee243782bf51 233 {
sjoerdbarts 6:ee243782bf51 234 SinTicker.attach(&MultiSin_motor2, 0.001);
sjoerdbarts 6:ee243782bf51 235 }
sjoerdbarts 6:ee243782bf51 236
sjoerdbarts 6:ee243782bf51 237 void HIDScope_Send()
sjoerdbarts 6:ee243782bf51 238 {
sjoerdbarts 6:ee243782bf51 239 scope.send();
sjoerdbarts 6:ee243782bf51 240 }
sjoerdbarts 6:ee243782bf51 241
sjoerdbarts 6:ee243782bf51 242
sjoerdbarts 6:ee243782bf51 243
s1600907 3:ac6f86c0db6e 244 int main() {
s1600907 3:ac6f86c0db6e 245 pc.baud(SERIAL_BAUD);
sjoerdbarts 6:ee243782bf51 246 pc.printf("\r\n ***THERMONUCLEAR WARFARE HAS COMMENCED*** \r\n");
sjoerdbarts 6:ee243782bf51 247
sjoerdbarts 6:ee243782bf51 248 // Set LED
sjoerdbarts 6:ee243782bf51 249 led=0;
sjoerdbarts 6:ee243782bf51 250 Ticker LedTicker;
sjoerdbarts 6:ee243782bf51 251 LedTicker.attach(SwitchLed,0.5f);
sjoerdbarts 6:ee243782bf51 252
sjoerdbarts 6:ee243782bf51 253 // Interrupt for buttons
sjoerdbarts 6:ee243782bf51 254 button1.fall(button1_switch);
sjoerdbarts 6:ee243782bf51 255 button2.fall(button2_switch);
sjoerdbarts 6:ee243782bf51 256
sjoerdbarts 6:ee243782bf51 257 pc.printf("\r\n ***************** \r\n");
sjoerdbarts 6:ee243782bf51 258 pc.printf("\r\n Press button 1 to start positioning the arms \r\n");
sjoerdbarts 6:ee243782bf51 259 pc.printf("\r\n Make sure both potentiometers are positioned halfway! \r\n");
sjoerdbarts 6:ee243782bf51 260 pc.printf("\r\n ***************** \r\n");
sjoerdbarts 6:ee243782bf51 261 while (button1_value == false)
sjoerdbarts 6:ee243782bf51 262 {
sjoerdbarts 6:ee243782bf51 263 // just wait
sjoerdbarts 6:ee243782bf51 264 }
sjoerdbarts 7:167b6dfdefee 265
sjoerdbarts 7:167b6dfdefee 266
sjoerdbarts 6:ee243782bf51 267 pc.printf("\r\n ***************** \r\n");
sjoerdbarts 6:ee243782bf51 268 pc.printf("\r\n Use potentiometers to control the motor arms \r\n");
sjoerdbarts 6:ee243782bf51 269 pc.printf("\r\n Pot 1 for motor 1 \r\n");
sjoerdbarts 6:ee243782bf51 270 pc.printf("\r\n Pot 2 for motor 2 \r\n");
sjoerdbarts 6:ee243782bf51 271 pc.printf("\r\n ***************** \r\n");
sjoerdbarts 6:ee243782bf51 272 pc.printf("\r\n Press button 2 to start the rest of the program \r\n");
sjoerdbarts 6:ee243782bf51 273 pc.printf("\r\n ***************** \r\n");
sjoerdbarts 6:ee243782bf51 274 // Start potmeter control
sjoerdbarts 6:ee243782bf51 275 PotControlTicker.attach(&PotControl,0.01f);
TimDouma 0:46a1642a10c8 276
sjoerdbarts 6:ee243782bf51 277 while (button2_value == false)
sjoerdbarts 6:ee243782bf51 278 {
sjoerdbarts 6:ee243782bf51 279 // just wait
sjoerdbarts 6:ee243782bf51 280 }
sjoerdbarts 7:167b6dfdefee 281 // When button is pressed detach PotControl and sendzeros to HIDScope
sjoerdbarts 6:ee243782bf51 282 PotControlTicker.detach();
sjoerdbarts 7:167b6dfdefee 283 // Reset the motor PWM values to zero
sjoerdbarts 7:167b6dfdefee 284 SetMotor(1, 0);
sjoerdbarts 7:167b6dfdefee 285 SetMotor(2, 0);
sjoerdbarts 7:167b6dfdefee 286 // wait a bit to make sure all movement is gone
sjoerdbarts 7:167b6dfdefee 287 wait(0.5);
sjoerdbarts 7:167b6dfdefee 288 // Reset the QUE encoder values so that the currect position is nog the 0 position
sjoerdbarts 7:167b6dfdefee 289 Motor1_ENC_CW.reset();
sjoerdbarts 7:167b6dfdefee 290 Motor1_ENC_CCW.reset();
sjoerdbarts 7:167b6dfdefee 291 Motor2_ENC_CW.reset();
sjoerdbarts 7:167b6dfdefee 292 Motor2_ENC_CCW.reset();
sjoerdbarts 7:167b6dfdefee 293
sjoerdbarts 7:167b6dfdefee 294 // Flush the HIDScope with zeros
sjoerdbarts 6:ee243782bf51 295 SendZerosTicker.attach(&SendZeros,0.001);
sjoerdbarts 7:167b6dfdefee 296 wait(1);
sjoerdbarts 7:167b6dfdefee 297 SendZerosTicker.detach();
sjoerdbarts 6:ee243782bf51 298
sjoerdbarts 6:ee243782bf51 299 // Program startup
sjoerdbarts 6:ee243782bf51 300 pc.printf("\r\n Starting multisin on motor 1 in: \r\n");
sjoerdbarts 6:ee243782bf51 301 pc.printf("\r\n 10 \r\n");
sjoerdbarts 6:ee243782bf51 302 wait(5);
sjoerdbarts 6:ee243782bf51 303 pc.printf("\r\n 5 \r\n");
sjoerdbarts 6:ee243782bf51 304 wait(1);
sjoerdbarts 6:ee243782bf51 305 pc.printf("\r\n 4 \r\n");
sjoerdbarts 6:ee243782bf51 306 wait(1);
sjoerdbarts 6:ee243782bf51 307 pc.printf("\r\n 3 \r\n");
sjoerdbarts 6:ee243782bf51 308 wait(1);
sjoerdbarts 6:ee243782bf51 309 pc.printf("\r\n 2 \r\n");
sjoerdbarts 6:ee243782bf51 310 wait(1);
sjoerdbarts 6:ee243782bf51 311 pc.printf("\r\n 1 \r\n");
sjoerdbarts 6:ee243782bf51 312 wait(1);
sjoerdbarts 6:ee243782bf51 313 pc.printf("\r\n 0 \r\n");
sjoerdbarts 6:ee243782bf51 314 pc.printf("\r\n Wait for the signal to complete \r\n");
sjoerdbarts 6:ee243782bf51 315 pc.printf("\r\n Press button 2 too continue afterwards \r\n");
sjoerdbarts 6:ee243782bf51 316 SinControl1();
sjoerdbarts 6:ee243782bf51 317
sjoerdbarts 6:ee243782bf51 318 while (button2_value == true)
sjoerdbarts 6:ee243782bf51 319 {
sjoerdbarts 6:ee243782bf51 320 // just wait
s1600907 5:a9da7bc89b24 321 }
sjoerdbarts 7:167b6dfdefee 322 // Flush the HIDScope with zeros
sjoerdbarts 7:167b6dfdefee 323 SendZerosTicker.attach(&SendZeros,0.001);
sjoerdbarts 7:167b6dfdefee 324 wait(1);
sjoerdbarts 7:167b6dfdefee 325 SendZerosTicker.detach();
TimDouma 2:bf1466ac4e6f 326
sjoerdbarts 7:167b6dfdefee 327 // Start multisine om motor 2 routine
sjoerdbarts 6:ee243782bf51 328 pc.printf("\r\n Starting multisin on motor 2 in: \r\n");
sjoerdbarts 6:ee243782bf51 329 pc.printf("\r\n 10 \r\n");
sjoerdbarts 6:ee243782bf51 330 wait(5);
sjoerdbarts 6:ee243782bf51 331 pc.printf("\r\n 5 \r\n");
sjoerdbarts 6:ee243782bf51 332 wait(1);
sjoerdbarts 6:ee243782bf51 333 pc.printf("\r\n 4 \r\n");
sjoerdbarts 6:ee243782bf51 334 wait(1);
sjoerdbarts 6:ee243782bf51 335 pc.printf("\r\n 3 \r\n");
sjoerdbarts 6:ee243782bf51 336 wait(1);
sjoerdbarts 6:ee243782bf51 337 pc.printf("\r\n 2 \r\n");
sjoerdbarts 6:ee243782bf51 338 wait(1);
sjoerdbarts 6:ee243782bf51 339 pc.printf("\r\n 1 \r\n");
sjoerdbarts 6:ee243782bf51 340 wait(1);
sjoerdbarts 6:ee243782bf51 341 pc.printf("\r\n 0 \r\n");
sjoerdbarts 6:ee243782bf51 342 SinControl2();
sjoerdbarts 6:ee243782bf51 343 wait(23);
sjoerdbarts 6:ee243782bf51 344 pc.printf("\r\n Program Finished, press reset to restart \r\n");
sjoerdbarts 6:ee243782bf51 345 while (true) {}
TimDouma 0:46a1642a10c8 346 }