jetfishteam / Mbed 2 deprecated brushless_test

unit test for brushless motors using a potentiometer and a castle creations esc with 0.5 center duty cycle

Dependencies:   ESC Servo mbed

Fork of brushlessmotor by jetfishteam

MainController.cpp@2:040b8c8f4f92, 2015-07-30 (annotated)

Committer:
demaille
Date:
Thu Jul 30 20:36:00 2015 +0000
Revision:
2:040b8c8f4f92
Parent:
0:187bb46ed128
Child:
3:605f216167f6
updated

Who changed what in which revision?

UserRevisionLine numberNew contents of line
demaille 0:187bb46ed128 1 #include "MainController.h"
demaille 0:187bb46ed128 2
demaille 0:187bb46ed128 3 MainController::MainController()
demaille 0:187bb46ed128 4 :ch1(p17,0.0636,0.1075), // yaw
demaille 0:187bb46ed128 5 ch2(p15,0.0623,0.1075), // pitch
demaille 0:187bb46ed128 6 ch3(p18,0.0614,0.1071), // amplitude
demaille 0:187bb46ed128 7 //ch4(p30,0.055,0.092), // adj
demaille 0:187bb46ed128 8 ch6(p16,0.0625,0.1055), // frequency
demaille 2:040b8c8f4f92 9 esc1(p25)
demaille 0:187bb46ed128 10 //ap(p25, p26)//,
demaille 0:187bb46ed128 11 //leftservo(p21),
demaille 0:187bb46ed128 12 //rightservo(p23)
demaille 0:187bb46ed128 13
demaille 0:187bb46ed128 14 {
demaille 0:187bb46ed128 15 wait_ms(50);
demaille 0:187bb46ed128 16 amp = 0.0;
demaille 0:187bb46ed128 17 frq = 1.0;
demaille 0:187bb46ed128 18 frqCmd = frq;
demaille 0:187bb46ed128 19 yaw = 0.5;
demaille 2:040b8c8f4f92 20 frqMin = 0.8; //hardcoded
demaille 0:187bb46ed128 21 frqMax = 1.8; //hardcoded
demaille 0:187bb46ed128 22 fullCycle = true;
demaille 2:040b8c8f4f92 23 raiser = 1.0;
demaille 0:187bb46ed128 24 alPi = 0.2/(0.2+0.001);//tf/(tf+Ts);
demaille 0:187bb46ed128 25
demaille 0:187bb46ed128 26 throttle_var = 0.5; //neutral position
demaille 0:187bb46ed128 27 esc1 = throttle_var;
demaille 0:187bb46ed128 28 esc1();
demaille 0:187bb46ed128 29 wait_ms(3000); //to arm brushless motor
demaille 0:187bb46ed128 30 //leftservo.calibrate(0.0008, 45);
demaille 0:187bb46ed128 31 //rightservo.calibrate(-0.0001, 45);
demaille 0:187bb46ed128 32
demaille 0:187bb46ed128 33 }
demaille 0:187bb46ed128 34
demaille 0:187bb46ed128 35 void MainController::control()
demaille 0:187bb46ed128 36 {
demaille 0:187bb46ed128 37 curTime = timer1.read();
demaille 0:187bb46ed128 38
demaille 0:187bb46ed128 39 // check every half cycle
demaille 2:040b8c8f4f92 40 if(curTime > (1/(2*frqCmd)-TOFF) ) {
demaille 0:187bb46ed128 41
demaille 2:040b8c8f4f92 42 if(curTime < 1/(2*frqCmd) ) {
demaille 2:040b8c8f4f92 43 ampNew = 0.0;
demaille 2:040b8c8f4f92 44 } else {
demaille 2:040b8c8f4f92 45 // read new yaw value every half cycle
demaille 2:040b8c8f4f92 46 yaw = this->calculateYaw(); // a value from -1 to 1
demaille 0:187bb46ed128 47
demaille 2:040b8c8f4f92 48 if(yaw < 0.1 && yaw > -0.1) { // eliminating noise around 0.0 +-0.1
demaille 2:040b8c8f4f92 49 yaw =0.0;
demaille 0:187bb46ed128 50 }
demaille 0:187bb46ed128 51
demaille 2:040b8c8f4f92 52 // Read volume and frequency only every full cycle
demaille 2:040b8c8f4f92 53 if( fullCycle ) {
demaille 2:040b8c8f4f92 54 //read other new inputs only at upward portion of full cycle
demaille 2:040b8c8f4f92 55 amp = this->calculateAmplitude(); // a value from 0 to 1
demaille 2:040b8c8f4f92 56 frq = this->calculateFrequency(); // a value from frqmin to frqmax
demaille 0:187bb46ed128 57
demaille 2:040b8c8f4f92 58 ampNew = amp;
demaille 0:187bb46ed128 59
demaille 2:040b8c8f4f92 60 if(yaw < 0.0) {
demaille 2:040b8c8f4f92 61 ampNew = (1.0+0.7*yaw)*amp;
demaille 2:040b8c8f4f92 62 }
demaille 2:040b8c8f4f92 63
demaille 2:040b8c8f4f92 64 fullCycle = false;
demaille 0:187bb46ed128 65
demaille 2:040b8c8f4f92 66 } else {
demaille 2:040b8c8f4f92 67 // reverse for the downward slope
demaille 2:040b8c8f4f92 68 amp = -amp;
demaille 2:040b8c8f4f92 69
demaille 2:040b8c8f4f92 70 ampNew = amp;
demaille 0:187bb46ed128 71
demaille 2:040b8c8f4f92 72 if(yaw > 0.0) {
demaille 2:040b8c8f4f92 73 ampNew = (1.0-0.7*yaw)*amp;
demaille 2:040b8c8f4f92 74 }
demaille 0:187bb46ed128 75
demaille 2:040b8c8f4f92 76 // use amp and frq from last cycle in order to make sure it is equalized
demaille 2:040b8c8f4f92 77 fullCycle = true;
demaille 2:040b8c8f4f92 78 }
demaille 2:040b8c8f4f92 79 // update the frequency that actually needs to be commanded
demaille 2:040b8c8f4f92 80 frqCmd = frq;
demaille 0:187bb46ed128 81
demaille 2:040b8c8f4f92 82 // read new yaw value every half cycle
demaille 2:040b8c8f4f92 83 //adj = this->calculateAdj(); // a value from 0 to 1
demaille 0:187bb46ed128 84
demaille 2:040b8c8f4f92 85 // for keeping track, calculate current volume storage, positive means on side is fuller, negative means other side is fuller
demaille 2:040b8c8f4f92 86 //volume = volChg + volume;
demaille 2:040b8c8f4f92 87 // rudder value used to define the trapezoid shape
demaille 2:040b8c8f4f92 88 raiser = 1.5; // varied from 1 to 5
demaille 2:040b8c8f4f92 89
demaille 2:040b8c8f4f92 90 //reset timer
demaille 2:040b8c8f4f92 91 timer1.reset();
demaille 2:040b8c8f4f92 92 curTime = 0.0;
demaille 2:040b8c8f4f92 93 }
demaille 0:187bb46ed128 94 }
demaille 0:187bb46ed128 95
demaille 0:187bb46ed128 96
demaille 0:187bb46ed128 97 // //Set Servo Values
demaille 0:187bb46ed128 98 pitch = this->calculatePitch();
demaille 0:187bb46ed128 99 // leftservo = pitch+0.03;
demaille 0:187bb46ed128 100 // rightservo = 1.0 - pitch;
demaille 0:187bb46ed128 101 // if (rightservo<0.03){
demaille 0:187bb46ed128 102 // rightservo = 0.03;
demaille 0:187bb46ed128 103 // }
demaille 0:187bb46ed128 104
demaille 0:187bb46ed128 105 dutyCycle = 0.5 + ampNew * saturate(raiser * sin( 2.0 * MATH_PI * frqCmd * curTime )); // add factor 4.0 to get a cut off sinus
demaille 0:187bb46ed128 106 //set brushless motor speed
demaille 0:187bb46ed128 107 throttle_var = dutyCycle;
demaille 0:187bb46ed128 108 esc1 = throttle_var;
demaille 0:187bb46ed128 109 esc1();
demaille 0:187bb46ed128 110 }
demaille 0:187bb46ed128 111
demaille 0:187bb46ed128 112 float MainController::calculateFrequency()
demaille 0:187bb46ed128 113 {
demaille 0:187bb46ed128 114 return ((frqMax - frqMin) * ch6.dutycyclescaledup() + frqMin);
demaille 0:187bb46ed128 115 //return (ch6.dutycyclescaledup());
demaille 0:187bb46ed128 116 }
demaille 0:187bb46ed128 117
demaille 0:187bb46ed128 118 float MainController::calculateAmplitude()
demaille 0:187bb46ed128 119 {
demaille 0:187bb46ed128 120 return (ch3.dutycyclescaledup() / 2.0);
demaille 0:187bb46ed128 121 }
demaille 0:187bb46ed128 122
demaille 0:187bb46ed128 123 float MainController::calculateYaw()
demaille 0:187bb46ed128 124 {
demaille 0:187bb46ed128 125 return (2.0*ch1.dutycyclescaledup() - 1.0); //maps from 0.0-1.0 to -1.0 - 1.0
demaille 0:187bb46ed128 126 //return (ch1.dutycyclescaledup());
demaille 0:187bb46ed128 127 }
demaille 0:187bb46ed128 128
demaille 0:187bb46ed128 129 float MainController::calculatePitch()
demaille 0:187bb46ed128 130 {
demaille 0:187bb46ed128 131 //float pitAvg = alPi * pitAvg+ (1.0 - alPi)*(ch2.dutycyclescaledup());
demaille 0:187bb46ed128 132 float pitAvg = (ch2.dutycyclescaledup());
demaille 0:187bb46ed128 133 return pitAvg;
demaille 0:187bb46ed128 134
demaille 0:187bb46ed128 135 }
demaille 0:187bb46ed128 136
demaille 0:187bb46ed128 137 //float MainController::calculateAdj()
demaille 0:187bb46ed128 138 //{
demaille 0:187bb46ed128 139 // return (ch4.dutycyclescaledup());
demaille 0:187bb46ed128 140 //}
demaille 0:187bb46ed128 141
demaille 0:187bb46ed128 142 void MainController::start()
demaille 0:187bb46ed128 143 {
demaille 0:187bb46ed128 144 timer1.start();
demaille 0:187bb46ed128 145
demaille 0:187bb46ed128 146 ticker1.attach(this, &MainController::control,0.001);
demaille 0:187bb46ed128 147 //Autopilot guardian
demaille 0:187bb46ed128 148 //ap.calibrate();
demaille 0:187bb46ed128 149 //ap.set2D();
demaille 0:187bb46ed128 150 //ap.setoff();
demaille 0:187bb46ed128 151
demaille 0:187bb46ed128 152 }
demaille 0:187bb46ed128 153
demaille 0:187bb46ed128 154 void MainController::stop()
demaille 0:187bb46ed128 155 {
demaille 0:187bb46ed128 156 timer1.stop();
demaille 0:187bb46ed128 157 ticker1.detach();
demaille 0:187bb46ed128 158 throttle_var = 0.5;
demaille 0:187bb46ed128 159 esc1 = throttle_var;
demaille 0:187bb46ed128 160 esc1();
demaille 0:187bb46ed128 161 }
demaille 0:187bb46ed128 162
demaille 0:187bb46ed128 163 float MainController::getDutyCycle()
demaille 0:187bb46ed128 164 {
demaille 0:187bb46ed128 165 return dutyCycle;
demaille 0:187bb46ed128 166 }
demaille 0:187bb46ed128 167
demaille 0:187bb46ed128 168 float MainController::getAmplitude()
demaille 0:187bb46ed128 169 {
demaille 0:187bb46ed128 170 return amp;
demaille 0:187bb46ed128 171 }
demaille 0:187bb46ed128 172
demaille 0:187bb46ed128 173
demaille 0:187bb46ed128 174 float MainController::getFrequency()
demaille 0:187bb46ed128 175 {
demaille 0:187bb46ed128 176 return frq;
demaille 0:187bb46ed128 177 }
demaille 0:187bb46ed128 178
demaille 0:187bb46ed128 179 float MainController::getYaw()
demaille 0:187bb46ed128 180 {
demaille 0:187bb46ed128 181 return yaw;
demaille 0:187bb46ed128 182 }
demaille 0:187bb46ed128 183
demaille 0:187bb46ed128 184 float MainController::getPitch()
demaille 0:187bb46ed128 185 {
demaille 0:187bb46ed128 186 return pitch;
demaille 0:187bb46ed128 187 }
demaille 0:187bb46ed128 188
demaille 0:187bb46ed128 189 //signum function
demaille 0:187bb46ed128 190 float MainController::signum(float input) //gives back the sign
demaille 0:187bb46ed128 191 {
demaille 0:187bb46ed128 192 if (input>0.0)
demaille 0:187bb46ed128 193 return 1.0;
demaille 0:187bb46ed128 194 else if (input<0.0)
demaille 0:187bb46ed128 195 return (-1.0);
demaille 0:187bb46ed128 196 else
demaille 0:187bb46ed128 197 return 0.0;
demaille 0:187bb46ed128 198 }
demaille 0:187bb46ed128 199
demaille 0:187bb46ed128 200 //saturate function
demaille 0:187bb46ed128 201 float MainController::saturate(float input) //saturates a value at 1.0 or -1.0
demaille 0:187bb46ed128 202 {
demaille 0:187bb46ed128 203 if (input > 1.0)
demaille 0:187bb46ed128 204 return (1.0);
demaille 0:187bb46ed128 205 else if (input < -1.0)
demaille 0:187bb46ed128 206 return (-1.0);
demaille 0:187bb46ed128 207 else
demaille 0:187bb46ed128 208 return input;
demaille 0:187bb46ed128 209 }