Enviolo (NuVinci) Road Bike Fully Automatic Shifter

Developed over 4 years and 14,000 miles for enviolo (NuVinci) CVT hubs using reed switches and excepts power data for fully auto shifting.

Jan 9, 2020

•

5722 views

•

1 respects

Components and supplies

Arduino Nano R3

HC-06 Bluetooth Module

Apps and platforms

BT Terminal/Graphics

Project description

Code

enviolo_AutoShifter_v2.3.0.ino

csharp

1/*
2  Bicycle Automatic Shifter Version 2 by Edward Rose 2020
3
4  Uses arduino bike speedometer w serial.print()code by Amanda Ghassaei 2012
5  http://www.instructables.com/id/Arduino-Bike-Speedometer/
6*/
7#include <SPI.h>
8#include  <NRFLite.h>
9#include <avdweb_AnalogReadFast.h>
10#include <PWMServo.h>
11#include  <EEPROM.h>
12
13PWMServo myservo;  // create servo object to control a servo
14//define  pins
15//V1
16//#define potpin A0                                   // analog  pin used to detect handlebar button press
17//#define derailSwitch A2                             //  pin connected to deraileur position reed switch
18//#define reed A3                                      //  pin connected to wheel reed switch
19//#define reed2 6                                      //  pin connected to cadence reed switch
20//V2
21#define derailSwitch 6                                //  pin connected to deraileur position reed switch
22#define reed A2                                      //  pin connected to wheel reed switch
23#define reed2 A3                                     //  pin connected to cadence reed switch
24#define buttonPinL 3                                  //  pin connected to left shifter button
25#define buttonPinR 2                                  //  pin connected to right shifter button
26#define lightSwitch 4                                 //  pin to operate rear light
27//BOTH
28int btLED = 5;                                       //  pin connected to BT LED
29//#define servopot A6                                   //  pin reading voltage of servos internal pot
30#define batVolt A7                                    //  Li-ion voltage via potential divider OLD 10k/ 3.9k NEW 3.3k/ 2.7k
31
32// changable  values stored in EEPROM some calculated on spreadsheet
33// use mode 5 to change  via Serial monitor or Bluetooth
34//Ratio set up
35byte DScalarB = 0;                                   //  big chainring
36byte DScalarS = 0;                                   // small chainring
37//Shifting
38int  ratioOffset = 0;                                // compensates for differances in  ratio profile due to position of hub interface
39int shiftSlope = 0;                                //  shift slope depends on hub under/overdrive ratio and servo position
40//Servo position  ajustment values
41int servoLowlimit = 0;                             // lower  value used in servopos constrain function
42int servoUpLimit = 0;                              //  upper value used in servopos constrain function
43//PID for cadence correction
44float  scalarP = 0;
45float scalarI = 0;
46//float scalarD = 500;                             //  never found D to be necessary
47//Power equation used to get multipler which =  ((sin(powerAv / powerDiv)) * powerMult) + powerAdd;
48int powerDiv = 0;
49byte  powerMult = 0;
50byte powerAdd = 0;
51//Distance measurment
52int cirWheel =  0;                                   // wheel circumference in cm for odometer
53int  firstMult = 200;                                // increase to 2000 if using servopos  540 - 2450
54
55int reedVal;                                         // status  of rear wheel reed switch
56int reedVal2;                                        //  status of crank reed switch
57long timer;                                          //  time for one full rotation (in ms)
58long timer2;                                         //  time for one full rotation (in ms)
59float Wheel;                                         //  outputs of reed switch timer
60int Cadence;
61int Speed;
62int Power;                                            //  power recieved from power meter
63float multipler;                                      //  used in main servopos equation to get correct cadence
64int errorC;                                           //  cadence error
65float prevErrorTotal;                                 // store  offSetI
66//float prevError;                                    // store prev errorC  for offSetD
67float offSetC;                                        // total constrained  cadence offset
68float offSetP;                                        // proportional  offset
69float offSetI;                                        // integral offset
70//float  offSetD;                                        // derivative offset
71int maxReedCounter  = 10;                              // min time (in ms) of one rotation (for debouncing)
72int  reedCounter;
73int reedCounter2;
74int buttonCounter = 0;                                //  push button press counter
75volatile int Counter = 0;
76//int buttons;
77bool  buttonL;                                         // left shifter button status
78bool  buttonR;                                         // right shifter button status
79int  servopos;                                         // value written to servo
80//int  preServopos;
81int switchState2 = 0;                                 // state of  mode switch2 derailleur position
82int DScalar;                                          //  sets the correct scalar depending on derailleur position
83int BTmode = 0;                                       //  read from bluetooth
84int mode = 3;                                         //  mode set by Bluetooth
85int cadenceSet;                                       //  cadence setting
86int setting = 0;                                      // gear/  cadence/ torque setting
87int prevCadenceSet = 60;                              //  delay gives time for the servo to move before being checked for error
88int lipoPercent;                                      // auto shifter battery percent approx
89int  lipoPercentP;                                     // power meter battery percent  approx
90int servovolt;                                        // voltage read  from servo pot by pin A6
91int servoposError;
92int zeroError = 0;                                    //  From power meter diffence between min value and sensorValue
93int torqueSet = 0;                                    // torque setting using shifter
94struct dataStruct  {                                   // data recived from power meter via nRF24L01
95  int value1;                                         // power from power meter
96  int value2;                                         // zeroError from power meter
97  int value3;                                         // lipoPercent from power  meter
98} myData;
99const int numReadings = 5;                            // power  average divide by 2 for time in secs
100int readings[numReadings];                            //  for power average code
101int index = 0;                                        //  for power average code
102int total = 0;                                        //  for power average code
103float powerAv = 0;                                    //  averaged power to avoid oscillations
104int codeTimer = 0;                                    //  triggerd when cadence reed switch closes
105int delayTime = 0;                                    //  time of a crank rotation
106int inputTorque = 0;
107int outputTorque = 0;
108int  offTimer = 0;                                     // save time since bike last moved
109int  lightMode = 0;                                    // save rear light setting
110int  inPut = 0;                                        // light setting input from serial
111int  BTinPut = 0;                                      // program mode serial input
112bool  btStatus = 0;                                    // status of BT LED
113unsigned  long distance = 0;                           // add up cm travelled
114unsigned  long odometer;                               // total distance (100th of a km) travelled,  stored in EEPROM
115NRFLite _radio;
116
117
118void setup() {
119  Serial.begin(9600);
120  _radio.init(0, 7, 10);          // radio id, CE pin, CSN pin
121
122  myservo.attach(SERVO_PIN_A);   // attaches the servo on pin 9 to the servo object
123  pinMode(reed, INPUT);
124  pinMode(reed2, INPUT_PULLUP);  // POWER LATCH
125  pinMode(derailSwitch, INPUT);
126  pinMode(batVolt, INPUT);
127  pinMode(btLED, INPUT);
128  //pinMode(servopot,  INPUT);
129  //V2 only
130  pinMode(buttonPinL, INPUT_PULLUP);
131  pinMode(buttonPinR,  INPUT_PULLUP);
132  pinMode(lightSwitch, INPUT);
133
134  EEPROM.get(20, DScalarB);
135  EEPROM.get(21, DScalarS);
136  EEPROM.get(22, ratioOffset);
137  EEPROM.get(24,  shiftSlope);
138  EEPROM.get(26, servoLowlimit);
139  EEPROM.get(28, servoUpLimit);
140  EEPROM.get(30, scalarP);
141  EEPROM.get(34, scalarI);
142  EEPROM.get(38, powerDiv);
143  EEPROM.get(40, powerMult);
144  EEPROM.get(41, powerAdd);
145  EEPROM.get(42,  cirWheel);
146
147  reedCounter = maxReedCounter;
148  reedCounter2 = maxReedCounter;
149  myservo.write(servoLowlimit);   // give servo a position while setup code finishes
150
151  inPut = EEPROM.read(15);       // get light setting from before last switched  off
152  while (inPut != lightMode) {
153    buttonPress();
154    lightMode ++;
155    if (lightMode > 7) {
156      lightMode = 0;
157    }
158  }
159  // TIMER SETUP-  the timer interrupt allows precise timed measurements of the reed switch
160  //  for more info about configuration of arduino timers see http://arduino.cc/playground/Code/Timer1
161
162  cli();//stop interrupts
163
164  //set timer2 interrupt at 1kHz
165  TCCR2A =  0;// set entire TCCR1A register to 0
166  TCCR2B = 0;// same for TCCR1B
167  TCNT2  = 0;
168  // set timer count for 1kHz increments
169  OCR2A = 249;// = (1/1000)  / (16*10^6)/(1000*64) - 1
170  // turn on CTC mode
171  TCCR2A |= (1 << WGM21);
172  // Set CS11 bit for 64 prescaler
173  TCCR2B |= (1 << CS22);
174  // enable timer  compare interrupt
175  TIMSK2 |= (1 << OCIE2A);
176
177  sei();//allow interrupts
178  //END TIMER SETUP
179}
180
181
182ISR(TIMER2_COMPA_vect) {                               //  Interrupt at freq of 1kHz to measure reed switch
183  reedVal = digitalRead(reed);                         // get status of wheel reed switch
184  if (reedVal) {                                       // if reed switch is closed
185    if (reedCounter  == 0) {                            // min time between pulses has passed
186      Wheel  = ((DScalar * firstMult) / float(timer));  // calculate rotation rate of wheel,  includes multipler for big/small chainrings
187      Speed = ((4817) / float(timer));
188      timer = 0;                                       // reset timer
189      reedCounter  = maxReedCounter;                    // reset reedCounter
190      distance = distance  + cirWheel;                  // add another wheel circumference to distance travelled
191    }
192  }
193  else {                                               // if reed  switch is open
194    if (reedCounter > 0) {                             // don't  let reedCounter go negative
195      reedCounter -= 1;                                //  decrement reedCounter
196    }
197  }
198  if (timer > 2000) {
199    Wheel = 0;
200    Speed = 0;                                         // if no new pulses from  reed switch tire is still, set mph to 0
201  }
202  else {
203    timer += 1;                                        //  increment timer
204  }
205
206  //TIMER1 CADENCE
207  reedVal2 = digitalRead(reed2);           // get status of crank reed switch
208  if (!reedVal2) {
209    if (reedCounter2  == 0) {
210      Cadence = (60000 / float(timer2));
211      codeTimer = 1;                    //  used to time shift at dead spot of pedal stroke
212      delayTime = timer2;
213      timer2 = 0;
214      reedCounter2 = maxReedCounter;
215    }
216  }
217  else  {
218    if (reedCounter2 > 0) {
219      reedCounter2 -= 1;
220    }
221  }
222  if (timer2 > 2000) {
223    Cadence = 0;
224  }
225  else {
226    timer2 += 1;
227  }
228  //V1 two buttons on one pin with resistors, analog read
229  /* buttons  = analogReadFast(potpin);          // check if a handlebar button has been pressed
230
231    if (buttons > 600)                          // time button press
232    {
233     buttonCounter ++;
234    }
235
236    else if (buttons < 400)
237    {
238     buttonCounter --;
239    }
240
241    else
242    {
243     Counter = buttonCounter;
244    }*/
245  // V2 one button per pin
246  buttonL = digitalRead(buttonPinL);
247  buttonR = digitalRead(buttonPinR);
248
249  if (buttonL == LOW)
250  {
251    buttonCounter  ++;
252  }
253  else {
254    if (buttonR == LOW)
255    {
256      buttonCounter  --;
257    }
258    else
259    {
260      Counter = buttonCounter;
261    }
262  }
263}
264void buttonPress() {                // press button on rear light
265  pinMode(lightSwitch, OUTPUT);
266  digitalWrite(lightSwitch, LOW);
267  delay(30);
268  pinMode(lightSwitch, INPUT);
269  delay(30);
270}
271
272void loop() {
273  //  time shift to happen at dead spots of pedal stroke
274  if (Cadence > 38) {
275    if (codeTimer) {
276      delay (delayTime * 0.2); //0.25
277      codeTimer  = 0;
278      mainCode();
279      delay (delayTime * 0.7);  //0.5
280      mainCode();
281    }
282  }
283  else {
284    mainCode();
285    delay (500);
286  }
287}
288void  mainCode() {
289  // power latch circiut switch off and save data
290  /*if (Speed  == 0) {
291    offTimer++;
292    }
293    else {
294    offTimer = 0;
295    }*/
296  // if BT LED connected to D5
297  if (Speed > 0) {
298    offTimer = 0;
299  }
300  else {
301    btStatus = digitalRead(btLED);
302    if (btStatus == LOW) {
303      offTimer++;
304    }
305    else {
306      offTimer = offTimer;
307    }
308  }
309
310  if (offTimer > 30) {                        // switch off after approx  secs
311    EEPROM.get(10, odometer);                 // read last saved 100th of  kms from EEPROM
312    odometer = odometer + (distance / 1000);  // divide by 1000  to convert cm to 100th of a km
313    EEPROM.put(10, odometer);                 //  save distance to odometer
314    pinMode(reed2, OUTPUT);                   // unlatch  power latch circuit
315    digitalWrite(reed2, LOW);
316    distance = 0;                             //  reset distance if Arduino doesn't switch off
317  }
318  // check for input from  bluetooth to change mode
319  if (Serial.available() > 0) {
320    BTmode = Serial.read();
321
322    if (BTmode == '1') {
323      mode = 1;
324      Serial.println("Manual");
325    }
326    else if (BTmode == '2') {
327      mode = 2;
328      Serial.println("CC  on");
329    }
330    else if (BTmode == '3') {
331      mode = 3;
332      Serial.println("PowerOffset");
333    }
334    else if (BTmode == '4') {
335      mode = 4;
336      Serial.println("PowerSlope");
337    }
338    else if (BTmode == '5') {
339      mode = 5;
340      Serial.println("Program  Mode");
341    }
342    else if (BTmode == '6') {
343      mode = 6;
344      Serial.println("Light  Control");
345    }
346    else if (BTmode == '7') {
347      mode = 7;
348      Serial.println("EEPROM  Read");
349    }
350    else if (BTmode == '8') {
351      mode = 8;
352      Serial.println("Servo  manaul control");
353    }
354  }
355  // change gear, cadence or torque setting
356  if (Counter > 50 && Counter <= 500 && setting  < 8) {
357    buttonCounter = 0;
358    setting ++;
359  }
360
361  else if (Counter < -50 && Counter >= -500 && setting  > -4) {
362    buttonCounter = 0;
363    setting  --;
364  }
365
366  else if (Counter  > 500 || Counter < -500) {
367    buttonCounter = 0;
368    setting  = 0;
369  }
370
371  // measure battery %
372  lipoPercent = ((analogReadFast(batVolt) * 0.55) - 385);    // give approx battery percentage but must be calibrated to the voltage divider  used
373
374  //data recived from power meter
375  while (_radio.hasData()) {
376    _radio.readData(&myData);
377
378    Power = myData.value1;
379    zeroError  = myData.value2;
380    lipoPercentP = myData.value3;
381  }
382
383  // get derailluer  position and calculate torque if required and power data available
384  switchState2  = digitalRead(derailSwitch);                 // check status of derailleur position  reed switch
385
386  if (switchState2 == HIGH)                                 //  derailleur position check, DScaler setting and torque calcs
387  {
388    (DScalar  = DScalarS);                                   // small chainring
389    //inputTorque  = ((Power * 9.5488)/(Cadence*1.89));        // calculate ratio chainring/sprocket,  example 34/18
390  }
391
392  else
393  {
394    (DScalar = DScalarB);                                   //  big chainring
395    //inputTorque = ((Power * 9.5488)/(Cadence*2.78));        //  calculate ratio chainring/sprocket, example 50/18
396  }
397  //outputTorque = ((Power  * 9.5488)/(Speed * 12.46));
398
399  if (mode == 7) // EEPROM Read
400  {
401    EEPROM.get(10,  odometer);
402    odometer = odometer + (distance / 1000);             // divide  by 1000 to convert cm to 100th of a km
403    Serial.print("Odometer ");
404    Serial.print(odometer  / 160);                        // divide by 100 for km, 160 for approx miles
405    Serial.println(" miles");
406    /*Serial.println("Press any key to continue");
407      while (!Serial.available()) delay(10);
408      inPut = Serial.read();*/
409    delay(5000);
410    mode = 3;
411  }
412
413  if (mode == 6) //Light control
414  {
415    Serial.println("Enter mode 1 - 7 or 0 for off");
416    while (Serial.available()  == 0) {}
417    inPut = (Serial.parseFloat());
418    if (inPut > 7) {
419      Serial.println("Error");
420    }
421    else {
422      EEPROM.write(15, inPut);
423      while (inPut != lightMode)  {
424        buttonPress();
425        Serial.println(lightMode);
426        lightMode  ++;
427        if (lightMode > 7) {
428          lightMode = 0;
429        }
430      }
431      mode = 3;
432    }
433  }
434  if (mode == 5) //Program mode
435  {
436    Serial.println();
437    Serial.println("Select value to change or 13  to exit");
438    Serial.print("1.  DScalarB       ");
439    Serial.println(DScalarB);
440    Serial.print("2.  DScalarS       ");
441    Serial.println(DScalarS);
442    Serial.print("3.  ratioOffset    ");
443    Serial.println(ratioOffset);
444    Serial.print("4.  shiftSlope     ");
445    Serial.println(shiftSlope);
446    Serial.print("5.  servoLowlimit  ");
447    Serial.println(servoLowlimit);
448    Serial.print("6.  servoUpLimit   ");
449    Serial.println(servoUpLimit);
450    Serial.print("7.  scalarP        ");
451    Serial.println(scalarP);
452    Serial.print("8.  scalarI        ");
453    Serial.println(scalarI);
454    Serial.print("9.  powerDiv       ");
455    Serial.println(powerDiv);
456    Serial.print("10. powerMult      ");
457    Serial.println(powerMult);
458    Serial.print("11. powerAdd       ");
459    Serial.println(powerAdd);
460    Serial.print("12. cirWheel       ");
461    Serial.println(cirWheel);
462    while (Serial.available() ==  0) {}
463    BTinPut = Serial.parseInt();
464
465
466    if (BTinPut == 1) {
467      Serial.println();
468      Serial.print("DScalarB Enter value ");
469      while  (Serial.available() == 0) {}
470      DScalarB = (Serial.parseInt());
471      EEPROM.put(20,  DScalarB);
472      Serial.println();
473      Serial.println(" Saved");
474    }
475    else if (BTinPut == 2) {
476      Serial.println();
477      Serial.print("DScalarS  Enter value ");
478      while (Serial.available() == 0) {}
479      DScalarS =  (Serial.parseInt());
480      EEPROM.put(21, DScalarS);
481      Serial.println();
482      Serial.println(" Saved");
483    }
484    else if (BTinPut == 3) {
485      Serial.println();
486      Serial.print("ratioOffset Enter value ");
487      while (Serial.available()  == 0) {}
488      ratioOffset = (Serial.parseInt());
489      EEPROM.put(22, ratioOffset);
490      Serial.println();
491      Serial.println(" Saved");
492    }
493    else  if (BTinPut == 4) {
494      Serial.println();
495      Serial.print("shiftSlope  Enter value ");
496      while (Serial.available() == 0) {}
497      shiftSlope  = (Serial.parseInt());
498      EEPROM.put(24, shiftSlope);
499      Serial.println();
500      Serial.println(" Saved");
501    }
502    else if (BTinPut == 5) {
503      Serial.println();
504      Serial.print("servoLowlimit Enter value ");
505      while (Serial.available()  == 0) {}
506      servoLowlimit = (Serial.parseInt());
507      EEPROM.put(26, servoLowlimit);
508      Serial.println();
509      Serial.println(" Saved");
510    }
511    else  if (BTinPut == 6) {
512      Serial.println();
513      Serial.print("servoUpLimit  Enter value ");
514      while (Serial.available() == 0) {}
515      servoUpLimit  = (Serial.parseInt());
516      EEPROM.put(28, servoUpLimit);
517      Serial.println();
518      Serial.println(" Saved");
519    }
520    else if (BTinPut == 7) {
521      Serial.println();
522      Serial.print("scalarP Enter value ");
523      while (Serial.available()  == 0) {}
524      scalarP = (Serial.parseFloat());
525      EEPROM.put(30, scalarP);
526      Serial.println();
527      Serial.println(" Saved");
528    }
529    else  if (BTinPut == 8) {
530      Serial.println();
531      Serial.print("scalarI Enter  value ");
532      while (Serial.available() == 0) {}
533      scalarI = (Serial.parseFloat());
534      EEPROM.put(34, scalarI);
535      Serial.println();
536      Serial.println("  Saved");
537    }
538    else if (BTinPut == 9) {
539      Serial.println();
540      Serial.print("powerDiv Enter value ");
541      while (Serial.available()  == 0) {}
542      powerDiv = (Serial.parseInt());
543      EEPROM.put(38, powerDiv);
544      Serial.println();
545      Serial.println(" Saved");
546    }
547    else  if (BTinPut == 10) {
548      Serial.println();
549      Serial.print("powerMult  Enter value ");
550      while (Serial.available() == 0) {}
551      powerMult  = (Serial.parseInt());
552      EEPROM.put(40, powerMult);
553      Serial.println();
554      Serial.println(" Saved");
555    }
556    else if (BTinPut == 11) {
557      Serial.println();
558      Serial.print("powerAdd Enter value ");
559      while  (Serial.available() == 0) {}
560      powerAdd = (Serial.parseInt());
561      EEPROM.put(41,  powerAdd);
562      Serial.println();
563      Serial.println(" Saved");
564    }
565    else if (BTinPut == 12) {
566      Serial.println();
567      Serial.print("cirWheel  Enter value ");
568      while (Serial.available() == 0) {}
569      cirWheel =  (Serial.parseInt());
570      EEPROM.put(42, cirWheel);
571      Serial.println();
572      Serial.println(" Saved");
573    }
574    else if (BTinPut == 13) {
575      mode = 3;
576      Serial.println("Return to run");
577    }
578  }
579  else if (mode == 8) //servo manual control
580  {
581    Serial.println("Use  buttons to move servo or enter a value");
582    Serial.println("Enter negative  number to exit");
583
584    while (Serial.available() == 0) {
585      if (buttonCounter  > 50) {
586        servopos ++;
587        buttonCounter = 0;
588        myservo.write(servopos);
589        Serial.println(servopos);
590      }
591
592      else if (buttonCounter  < -50) {
593        servopos  --;
594        buttonCounter = 0;
595        myservo.write(servopos);
596        Serial.println(servopos);
597      }
598    }
599    servopos = (Serial.parseInt());
600    if (servopos >= 0) {
601      myservo.write(servopos);
602      Serial.println(servopos);
603    }
604    else {
605      mode = 3;
606      Serial.println("Return to run");
607    }
608  }
609  else if (mode == 1) //manual
610  { /*// alternative manaul
611
612      if (buttonCounter > 80 && servopos  < servoUpLimit){
613         servopos  ++;
614         buttonCounter = 0;  }
615
616      else if (buttonCounter < -80  && servopos > servoLowlimit){
617         servopos  --;
618         buttonCounter  = 0;  }
619
620      myservo.write(servopos);
621    */
622    if (setting == 8)
623    {
624      servopos = (servoUpLimit);
625    }
626
627    else if (setting <=  7 && setting >= -3)
628    {
629      servopos = ((((servoUpLimit - servoLowlimit)  / 12) * (setting + 4)) + servoLowlimit);
630    }
631
632    else if (setting ==  -4)
633    {
634      servopos = (servoLowlimit);
635    }
636    myservo.write(servopos);
637
638    //servovolt = analogRead(servopot);                     // read servos internal  pot to get real position
639    //servoposError = servovolt - (((servopos - 2) *  2.84) + 83); // calculate error in servos position
640
641    Serial.print("E");
642    Serial.print(Power);
643    Serial.print(", ");
644    Serial.print(Cadence);
645    Serial.print(", ");
646    Serial.print(Speed);
647    Serial.print(", ");
648    Serial.print(setting);
649    Serial.print(", ");
650    Serial.print(servopos);    // (servopos);
651    Serial.print(", ");
652    Serial.print(lipoPercentP);    //(servoposError);
653    Serial.print(", ");
654    Serial.println(zeroError);
655
656  }
657
658  else if (Power == 0 || mode == 2) // Constant cadence
659  {
660    if  (setting == 4)
661    {
662      cadenceSet = 50;
663    }
664
665    else if (setting  == 3)
666    {
667      cadenceSet = 60;
668    }
669
670    else if (setting ==  2)
671    {
672      cadenceSet = 65;
673    }
674
675    else if (setting == 1)
676    {
677      cadenceSet = 68;
678    }
679
680    else if (setting == 0)
681    {
682      cadenceSet = 72;
683    }
684
685    else if (setting == -1)
686    {
687      cadenceSet = 75;
688    }
689
690    else if (setting == -2)
691    {
692      cadenceSet = 80;
693    }
694
695    else if (setting == -3)
696    {
697      cadenceSet = 90;
698    }
699
700    else if (setting == -4)
701    {
702      cadenceSet = 100;
703    }
704    multipler = shiftSlope / cadenceSet;
705
706    errorC = (Cadence - prevCadenceSet);
707
708    // calculate offsetP
709    if  ((Cadence > (cadenceSet + 10)) && (Cadence < 200))
710    {
711      offSetP = (multipler  / scalarP);
712    }
713    else if (Cadence < 38)
714    {
715      offSetP = 0;
716    }
717    else {
718      offSetP = (sin((errorC) * 0.15)) * (multipler / scalarP);
719    }
720
721    // calculate offsetI
722    if (Cadence < 38)
723    {
724      offSetI  = 0;
725    }
726    else {
727      offSetI = constrain ((prevErrorTotal + (errorC  * scalarI)), -1.5, 3); //-1.5, 1.5
728    }
729    prevErrorTotal = offSetI;
730    /*
731      // calculate offsetD
732      if (Cadence < 38)
733         {offSetI  = 0;}
734      else {
735      offSetD = constrain(((errorC - prevError) * scalarD),-3,15);
736                   }
737      prevError = errorC;
738    */
739    offSetC = offSetP  + offSetI; // + offSetD;
740
741    servopos = constrain((ratioOffset + (Wheel *  (multipler + offSetC))), servoLowlimit, servoUpLimit); // calculates servopos, sets  the limits on under/overdrive.
742    //multiplier sets the cadence, fixed offset  compensates for difference in ratio profile.
743    myservo.write(servopos);
744
745    //CALIBRATE
746    /*servovolt = analogRead(servopot);
747      servoposError  = servovolt-(((servoposD-2)*2.84)+83);
748
749      Serial.print("E");                //  send values via bluetooth or serial monitor for graphing or logging.
750      Serial.print(Power);            // "E" enables an app called Bluetooth Graphics to recognise it as  data.
751      Serial.print(", ");
752      Serial.print(Cadence);
753      Serial.print(",  ");
754      Serial.print(Speed);
755      Serial.print(", ");
756      Serial.print(switchState2);
757      Serial.print(", ");
758      Serial.print(servoposD);     //cadenceSet
759      Serial.print(", ");
760      Serial.print(servoposError);   //lipoPercentP
761      Serial.print(", ");
762      Serial.println(zeroError);
763    */
764
765    Serial.print("E");                // send values via bluetooth or serial monitor  for graphing or logging.
766    Serial.print(Power);            // "E" enables  an app called Bluetooth Graphics to recognise it as data.
767    Serial.print(",  ");
768    Serial.print(Cadence);
769    Serial.print(", ");
770    Serial.print(Speed);
771    Serial.print(", ");
772    Serial.print(setting);  // lipoPercent
773    Serial.print(",  ");
774    Serial.print(lipoPercent);    //cadenceSet, servopos, outputTorque
775    Serial.print(", ");
776    Serial.print(lipoPercentP);
777    Serial.print(",  ");
778    Serial.println(zeroError);
779    //APP
780    /*
781      Serial.print("  "); //Prevents The operation * cannot accept the arguments: , [*empty-string*],  [-0.24]
782      Serial.print(Power);
783      Serial.print(",");
784      Serial.print(Cadence);
785      Serial.print(",");
786      Serial.print(Speed);
787      Serial.print(",");
788      Serial.print(cadenceSet);
789      Serial.print(",");
790      Serial.print(offSetC);
791      Serial.print(",");
792      Serial.print(setting);
793      Serial.print(",");
794      Serial.print(lipoPercent);
795      Serial.print(",");
796      Serial.print(lipoPercentP);
797      Serial.print(",");
798      Serial.print(zeroError);
799      Serial.print(",");
800      Serial.print(inputTorque);
801      Serial.print(",");
802      Serial.println(outputTorque);  */
803
804    prevCadenceSet = cadenceSet;     //store prev cadence setpoint
805    // preServoposD = servoposD;
806  }
807
808  else if (Power > 0 && mode == 3)  // Power1 buttons change powerAdd
809  {
810    // average power 2.5 secs worked  best less results in oscillations, more to much lag
811    total = total - readings[index];
812    readings[index] = Power;
813    total = total + readings[index];
814    index  = index + 1;
815    if (index >= numReadings)
816      index = 0;
817    powerAv  = total / numReadings;
818
819    multipler = ((sin(powerAv / powerDiv)) * powerMult)  + (setting + powerAdd);
820    cadenceSet = shiftSlope / multipler;
821
822    errorC  = (Cadence - cadenceSet);
823    // calculate offsetP
824    if ((Cadence > (cadenceSet  + 10)) && (Cadence < 200))
825    {
826      offSetP = (multipler / 11);
827    }
828    else if (Cadence < 38)
829    {
830      offSetP = 0;
831    }
832    else {
833      offSetP = (sin((errorC) * 0.15)) * (multipler / 12);
834    }
835
836    //  calculate offsetI
837    if (Cadence < 38)
838    {
839      offSetI = 0;
840    }
841    else {
842      offSetI = constrain ((prevErrorTotal + (errorC * scalarI)),  -1.5, 1.5);
843    }
844    prevErrorTotal = offSetI;
845
846    offSetC = offSetP  + offSetI; // + offSetD;
847
848    servopos = constrain((ratioOffset + (Wheel *  (multipler + offSetC))), servoLowlimit, servoUpLimit);
849
850    myservo.write(servopos);
851
852    //CALIBRATE
853    /*servovolt = analogRead(servopot);
854      servoposError  = servovolt-(((servoposD-2)*2.84)+83);
855
856      Serial.print("E");                //  send values via bluetooth or serial monitor for graphing or logging.
857      Serial.print(Power);            // "E" enables an app called Bluetooth Graphics to recognise it as  data.
858      Serial.print(", ");
859      Serial.print(Cadence);
860      Serial.print(",  ");
861      Serial.print(Speed);
862      Serial.print(", ");
863      Serial.print(switchState2);
864      Serial.print(", ");
865      Serial.print(servoposD);     //cadenceSet
866      Serial.print(", ");
867      Serial.print(servoposError);   //lipoPercentP
868      Serial.print(", ");
869      Serial.println(zeroError);
870    */
871    Serial.print("E");
872    Serial.print(Power);
873    Serial.print(", ");
874    Serial.print(Cadence);
875    Serial.print(", ");
876    Serial.print(Speed);
877    Serial.print(", ");
878    Serial.print(setting);     // lipoPercent,inputTorque
879    Serial.print(",  ");
880    Serial.print(lipoPercent);    //cadenceSet, servopos, outputTorque
881    Serial.print(", ");
882    Serial.print(lipoPercentP);
883    Serial.print(",  ");
884    Serial.println(zeroError);
885
886    /* //APP
887      Serial.print("  "); //Prevents The operation * cannot accept the arguments: , [*empty-string*],  [-0.24]
888      Serial.print(Power);
889      Serial.print(",");
890      Serial.print(Cadence);
891      Serial.print(",");
892      Serial.print(Speed);
893      Serial.print(",");
894      Serial.print(cadenceSet);
895      Serial.print(",");
896      Serial.print(offSetC);
897      Serial.print(",");
898      Serial.print(setting);
899      Serial.print(",");
900      Serial.print(lipoPercent);
901      Serial.print(",");
902      Serial.print(lipoPercentP);
903      Serial.print(",");
904      Serial.print(zeroError);
905      Serial.print(",");
906      Serial.print(inputTorque);
907      Serial.print(",");
908      Serial.println(outputTorque);
909    */
910  }
911
912  else if (Power > 0 && mode == 4) // Power2 buttons change  powerMult
913  {
914    // average power
915    total = total - readings[index];
916    readings[index] = Power;
917    total = total + readings[index];
918    index  = index + 1;
919    if (index >= numReadings)
920      index = 0;
921    powerAv  = total / numReadings;
922
923    multipler = ((sin(powerAv / powerDiv)) * (setting  + powerMult)) + powerAdd;
924    cadenceSet = shiftSlope / multipler;
925
926    errorC  = (Cadence - cadenceSet);
927    // calculate offsetP
928    if ((Cadence > (cadenceSet  + 10)) && (Cadence < 200))
929    {
930      offSetP = (multipler / 11);
931    }
932    else if (Cadence < 38)
933    {
934      offSetP = 0;
935    }
936    else {
937      offSetP = (sin((errorC) * 0.15)) * (multipler / 12);
938    }
939
940    //  calculate offsetI
941    if (Cadence < 38)
942    {
943      offSetI = 0;
944    }
945    else {
946      offSetI = constrain ((prevErrorTotal + (errorC * scalarI)),  -1.5, 1.5);
947    }
948    prevErrorTotal = offSetI;
949
950    offSetC = offSetP  + offSetI; // + offSetD;
951
952    servopos = constrain((ratioOffset + (Wheel *  (multipler + offSetC))), servoLowlimit, servoUpLimit);
953
954    myservo.write(servopos);
955
956    Serial.print("E");
957    Serial.print(Power);
958    Serial.print(", ");
959    Serial.print(Cadence);
960    Serial.print(", ");
961    Serial.print(Speed);
962    Serial.print(", ");
963    Serial.print(setting);     // lipoPercent,inputTorque
964    Serial.print(", ");
965    Serial.print(lipoPercent);    //cadenceSet, servopos,  outputTorque
966    Serial.print(", ");
967    Serial.print(lipoPercentP);
968    Serial.print(", ");
969    Serial.println(zeroError);
970    /*
971      Serial.print("E");
972      Serial.print(Power);
973      Serial.print(", ");
974      Serial.print(Cadence);
975      Serial.print(", ");
976      Serial.print(Speed);
977      Serial.print(",  ");
978      Serial.print(setting);     //lipoPercent
979      Serial.print(",  ");
980      Serial.print(cadenceSet);     //servopos
981      Serial.print(",  ");
982      Serial.print(offSetC);   //lipoPercentP
983      Serial.print(",  ");
984      Serial.println(zeroError);
985
986      //APP
987      Serial.print("  "); //Prevents The operation * cannot accept the arguments: , [*empty-string*],  [-0.24]
988      Serial.print(Power);
989      Serial.print(",");
990      Serial.print(Cadence);
991      Serial.print(",");
992      Serial.print(Speed);
993      Serial.print(",");
994      Serial.print(cadenceSet);
995      Serial.print(",");
996      Serial.print(offSetC);
997      Serial.print(",");
998      Serial.print(setting);
999      Serial.print(",");
1000      Serial.print(lipoPercent);
1001      Serial.print(",");
1002      Serial.print(lipoPercentP);
1003      Serial.print(",");
1004      Serial.print(zeroError);
1005      Serial.print(",");
1006      Serial.print(inputTorque);
1007      Serial.print(",");
1008      Serial.println(outputTorque);
1009    */
1010  }
1011}
1012

/*
  Bicycle Automatic Shifter Version 2 by Edward Rose 2020

  Uses arduino bike speedometer w serial.print()code by Amanda Ghassaei 2012
  http://www.instructables.com/id/Arduino-Bike-Speedometer/
*/
#include <SPI.h>
#include  <NRFLite.h>
#include <avdweb_AnalogReadFast.h>
#include <PWMServo.h>
#include  <EEPROM.h>

PWMServo myservo;  // create servo object to control a servo
//define  pins
//V1
//#define potpin A0                                   // analog  pin used to detect handlebar button press
//#define derailSwitch A2                             //  pin connected to deraileur position reed switch
//#define reed A3                                      //  pin connected to wheel reed switch
//#define reed2 6                                      //  pin connected to cadence reed switch
//V2
#define derailSwitch 6                                //  pin connected to deraileur position reed switch
#define reed A2                                      //  pin connected to wheel reed switch
#define reed2 A3                                     //  pin connected to cadence reed switch
#define buttonPinL 3                                  //  pin connected to left shifter button
#define buttonPinR 2                                  //  pin connected to right shifter button
#define lightSwitch 4                                 //  pin to operate rear light
//BOTH
int btLED = 5;                                       //  pin connected to BT LED
//#define servopot A6                                   //  pin reading voltage of servos internal pot
#define batVolt A7                                    //  Li-ion voltage via potential divider OLD 10k/ 3.9k NEW 3.3k/ 2.7k

// changable  values stored in EEPROM some calculated on spreadsheet
// use mode 5 to change  via Serial monitor or Bluetooth
//Ratio set up
byte DScalarB = 0;                                   //  big chainring
byte DScalarS = 0;                                   // small chainring
//Shifting
int  ratioOffset = 0;                                // compensates for differances in  ratio profile due to position of hub interface
int shiftSlope = 0;                                //  shift slope depends on hub under/overdrive ratio and servo position
//Servo position  ajustment values
int servoLowlimit = 0;                             // lower  value used in servopos constrain function
int servoUpLimit = 0;                              //  upper value used in servopos constrain function
//PID for cadence correction
float  scalarP = 0;
float scalarI = 0;
//float scalarD = 500;                             //  never found D to be necessary
//Power equation used to get multipler which =  ((sin(powerAv / powerDiv)) * powerMult) + powerAdd;
int powerDiv = 0;
byte  powerMult = 0;
byte powerAdd = 0;
//Distance measurment
int cirWheel =  0;                                   // wheel circumference in cm for odometer
int  firstMult = 200;                                // increase to 2000 if using servopos  540 - 2450

int reedVal;                                         // status  of rear wheel reed switch
int reedVal2;                                        //  status of crank reed switch
long timer;                                          //  time for one full rotation (in ms)
long timer2;                                         //  time for one full rotation (in ms)
float Wheel;                                         //  outputs of reed switch timer
int Cadence;
int Speed;
int Power;                                            //  power recieved from power meter
float multipler;                                      //  used in main servopos equation to get correct cadence
int errorC;                                           //  cadence error
float prevErrorTotal;                                 // store  offSetI
//float prevError;                                    // store prev errorC  for offSetD
float offSetC;                                        // total constrained  cadence offset
float offSetP;                                        // proportional  offset
float offSetI;                                        // integral offset
//float  offSetD;                                        // derivative offset
int maxReedCounter  = 10;                              // min time (in ms) of one rotation (for debouncing)
int  reedCounter;
int reedCounter2;
int buttonCounter = 0;                                //  push button press counter
volatile int Counter = 0;
//int buttons;
bool  buttonL;                                         // left shifter button status
bool  buttonR;                                         // right shifter button status
int  servopos;                                         // value written to servo
//int  preServopos;
int switchState2 = 0;                                 // state of  mode switch2 derailleur position
int DScalar;                                          //  sets the correct scalar depending on derailleur position
int BTmode = 0;                                       //  read from bluetooth
int mode = 3;                                         //  mode set by Bluetooth
int cadenceSet;                                       //  cadence setting
int setting = 0;                                      // gear/  cadence/ torque setting
int prevCadenceSet = 60;                              //  delay gives time for the servo to move before being checked for error
int lipoPercent;                                      // auto shifter battery percent approx
int  lipoPercentP;                                     // power meter battery percent  approx
int servovolt;                                        // voltage read  from servo pot by pin A6
int servoposError;
int zeroError = 0;                                    //  From power meter diffence between min value and sensorValue
int torqueSet = 0;                                    // torque setting using shifter
struct dataStruct  {                                   // data recived from power meter via nRF24L01
  int value1;                                         // power from power meter
  int value2;                                         // zeroError from power meter
  int value3;                                         // lipoPercent from power  meter
} myData;
const int numReadings = 5;                            // power  average divide by 2 for time in secs
int readings[numReadings];                            //  for power average code
int index = 0;                                        //  for power average code
int total = 0;                                        //  for power average code
float powerAv = 0;                                    //  averaged power to avoid oscillations
int codeTimer = 0;                                    //  triggerd when cadence reed switch closes
int delayTime = 0;                                    //  time of a crank rotation
int inputTorque = 0;
int outputTorque = 0;
int  offTimer = 0;                                     // save time since bike last moved
int  lightMode = 0;                                    // save rear light setting
int  inPut = 0;                                        // light setting input from serial
int  BTinPut = 0;                                      // program mode serial input
bool  btStatus = 0;                                    // status of BT LED
unsigned  long distance = 0;                           // add up cm travelled
unsigned  long odometer;                               // total distance (100th of a km) travelled,  stored in EEPROM
NRFLite _radio;


void setup() {
  Serial.begin(9600);
  _radio.init(0, 7, 10);          // radio id, CE pin, CSN pin

  myservo.attach(SERVO_PIN_A);   // attaches the servo on pin 9 to the servo object
  pinMode(reed, INPUT);
  pinMode(reed2, INPUT_PULLUP);  // POWER LATCH
  pinMode(derailSwitch, INPUT);
  pinMode(batVolt, INPUT);
  pinMode(btLED, INPUT);
  //pinMode(servopot,  INPUT);
  //V2 only
  pinMode(buttonPinL, INPUT_PULLUP);
  pinMode(buttonPinR,  INPUT_PULLUP);
  pinMode(lightSwitch, INPUT);

  EEPROM.get(20, DScalarB);
  EEPROM.get(21, DScalarS);
  EEPROM.get(22, ratioOffset);
  EEPROM.get(24,  shiftSlope);
  EEPROM.get(26, servoLowlimit);
  EEPROM.get(28, servoUpLimit);
  EEPROM.get(30, scalarP);
  EEPROM.get(34, scalarI);
  EEPROM.get(38, powerDiv);
  EEPROM.get(40, powerMult);
  EEPROM.get(41, powerAdd);
  EEPROM.get(42,  cirWheel);

  reedCounter = maxReedCounter;
  reedCounter2 = maxReedCounter;
  myservo.write(servoLowlimit);   // give servo a position while setup code finishes

  inPut = EEPROM.read(15);       // get light setting from before last switched  off
  while (inPut != lightMode) {
    buttonPress();
    lightMode ++;
    if (lightMode > 7) {
      lightMode = 0;
    }
  }
  // TIMER SETUP-  the timer interrupt allows precise timed measurements of the reed switch
  //  for more info about configuration of arduino timers see http://arduino.cc/playground/Code/Timer1

  cli();//stop interrupts

  //set timer2 interrupt at 1kHz
  TCCR2A =  0;// set entire TCCR1A register to 0
  TCCR2B = 0;// same for TCCR1B
  TCNT2  = 0;
  // set timer count for 1kHz increments
  OCR2A = 249;// = (1/1000)  / (16*10^6)/(1000*64) - 1
  // turn on CTC mode
  TCCR2A |= (1 << WGM21);
  // Set CS11 bit for 64 prescaler
  TCCR2B |= (1 << CS22);
  // enable timer  compare interrupt
  TIMSK2 |= (1 << OCIE2A);

  sei();//allow interrupts
  //END TIMER SETUP
}


ISR(TIMER2_COMPA_vect) {                               //  Interrupt at freq of 1kHz to measure reed switch
  reedVal = digitalRead(reed);                         // get status of wheel reed switch
  if (reedVal) {                                       // if reed switch is closed
    if (reedCounter  == 0) {                            // min time between pulses has passed
      Wheel  = ((DScalar * firstMult) / float(timer));  // calculate rotation rate of wheel,  includes multipler for big/small chainrings
      Speed = ((4817) / float(timer));
      timer = 0;                                       // reset timer
      reedCounter  = maxReedCounter;                    // reset reedCounter
      distance = distance  + cirWheel;                  // add another wheel circumference to distance travelled
    }
  }
  else {                                               // if reed  switch is open
    if (reedCounter > 0) {                             // don't  let reedCounter go negative
      reedCounter -= 1;                                //  decrement reedCounter
    }
  }
  if (timer > 2000) {
    Wheel = 0;
    Speed = 0;                                         // if no new pulses from  reed switch tire is still, set mph to 0
  }
  else {
    timer += 1;                                        //  increment timer
  }

  //TIMER1 CADENCE
  reedVal2 = digitalRead(reed2);           // get status of crank reed switch
  if (!reedVal2) {
    if (reedCounter2  == 0) {
      Cadence = (60000 / float(timer2));
      codeTimer = 1;                    //  used to time shift at dead spot of pedal stroke
      delayTime = timer2;
      timer2 = 0;
      reedCounter2 = maxReedCounter;
    }
  }
  else  {
    if (reedCounter2 > 0) {
      reedCounter2 -= 1;
    }
  }
  if (timer2 > 2000) {
    Cadence = 0;
  }
  else {
    timer2 += 1;
  }
  //V1 two buttons on one pin with resistors, analog read
  /* buttons  = analogReadFast(potpin);          // check if a handlebar button has been pressed

    if (buttons > 600)                          // time button press
    {
     buttonCounter ++;
    }

    else if (buttons < 400)
    {
     buttonCounter --;
    }

    else
    {
     Counter = buttonCounter;
    }*/
  // V2 one button per pin
  buttonL = digitalRead(buttonPinL);
  buttonR = digitalRead(buttonPinR);

  if (buttonL == LOW)
  {
    buttonCounter  ++;
  }
  else {
    if (buttonR == LOW)
    {
      buttonCounter  --;
    }
    else
    {
      Counter = buttonCounter;
    }
  }
}
void buttonPress() {                // press button on rear light
  pinMode(lightSwitch, OUTPUT);
  digitalWrite(lightSwitch, LOW);
  delay(30);
  pinMode(lightSwitch, INPUT);
  delay(30);
}

void loop() {
  //  time shift to happen at dead spots of pedal stroke
  if (Cadence > 38) {
    if (codeTimer) {
      delay (delayTime * 0.2); //0.25
      codeTimer  = 0;
      mainCode();
      delay (delayTime * 0.7);  //0.5
      mainCode();
    }
  }
  else {
    mainCode();
    delay (500);
  }
}
void  mainCode() {
  // power latch circiut switch off and save data
  /*if (Speed  == 0) {
    offTimer++;
    }
    else {
    offTimer = 0;
    }*/
  // if BT LED connected to D5
  if (Speed > 0) {
    offTimer = 0;
  }
  else {
    btStatus = digitalRead(btLED);
    if (btStatus == LOW) {
      offTimer++;
    }
    else {
      offTimer = offTimer;
    }
  }

  if (offTimer > 30) {                        // switch off after approx  secs
    EEPROM.get(10, odometer);                 // read last saved 100th of  kms from EEPROM
    odometer = odometer + (distance / 1000);  // divide by 1000  to convert cm to 100th of a km
    EEPROM.put(10, odometer);                 //  save distance to odometer
    pinMode(reed2, OUTPUT);                   // unlatch  power latch circuit
    digitalWrite(reed2, LOW);
    distance = 0;                             //  reset distance if Arduino doesn't switch off
  }
  // check for input from  bluetooth to change mode
  if (Serial.available() > 0) {
    BTmode = Serial.read();

    if (BTmode == '1') {
      mode = 1;
      Serial.println("Manual");
    }
    else if (BTmode == '2') {
      mode = 2;
      Serial.println("CC  on");
    }
    else if (BTmode == '3') {
      mode = 3;
      Serial.println("PowerOffset");
    }
    else if (BTmode == '4') {
      mode = 4;
      Serial.println("PowerSlope");
    }
    else if (BTmode == '5') {
      mode = 5;
      Serial.println("Program  Mode");
    }
    else if (BTmode == '6') {
      mode = 6;
      Serial.println("Light  Control");
    }
    else if (BTmode == '7') {
      mode = 7;
      Serial.println("EEPROM  Read");
    }
    else if (BTmode == '8') {
      mode = 8;
      Serial.println("Servo  manaul control");
    }
  }
  // change gear, cadence or torque setting
  if (Counter > 50 && Counter <= 500 && setting  < 8) {
    buttonCounter = 0;
    setting ++;
  }

  else if (Counter < -50 && Counter >= -500 && setting  > -4) {
    buttonCounter = 0;
    setting  --;
  }

  else if (Counter  > 500 || Counter < -500) {
    buttonCounter = 0;
    setting  = 0;
  }

  // measure battery %
  lipoPercent = ((analogReadFast(batVolt) * 0.55) - 385);    // give approx battery percentage but must be calibrated to the voltage divider  used

  //data recived from power meter
  while (_radio.hasData()) {
    _radio.readData(&myData);

    Power = myData.value1;
    zeroError  = myData.value2;
    lipoPercentP = myData.value3;
  }

  // get derailluer  position and calculate torque if required and power data available
  switchState2  = digitalRead(derailSwitch);                 // check status of derailleur position  reed switch

  if (switchState2 == HIGH)                                 //  derailleur position check, DScaler setting and torque calcs
  {
    (DScalar  = DScalarS);                                   // small chainring
    //inputTorque  = ((Power * 9.5488)/(Cadence*1.89));        // calculate ratio chainring/sprocket,  example 34/18
  }

  else
  {
    (DScalar = DScalarB);                                   //  big chainring
    //inputTorque = ((Power * 9.5488)/(Cadence*2.78));        //  calculate ratio chainring/sprocket, example 50/18
  }
  //outputTorque = ((Power  * 9.5488)/(Speed * 12.46));

  if (mode == 7) // EEPROM Read
  {
    EEPROM.get(10,  odometer);
    odometer = odometer + (distance / 1000);             // divide  by 1000 to convert cm to 100th of a km
    Serial.print("Odometer ");
    Serial.print(odometer  / 160);                        // divide by 100 for km, 160 for approx miles
    Serial.println(" miles");
    /*Serial.println("Press any key to continue");
      while (!Serial.available()) delay(10);
      inPut = Serial.read();*/
    delay(5000);
    mode = 3;
  }

  if (mode == 6) //Light control
  {
    Serial.println("Enter mode 1 - 7 or 0 for off");
    while (Serial.available()  == 0) {}
    inPut = (Serial.parseFloat());
    if (inPut > 7) {
      Serial.println("Error");
    }
    else {
      EEPROM.write(15, inPut);
      while (inPut != lightMode)  {
        buttonPress();
        Serial.println(lightMode);
        lightMode  ++;
        if (lightMode > 7) {
          lightMode = 0;
        }
      }
      mode = 3;
    }
  }
  if (mode == 5) //Program mode
  {
    Serial.println();
    Serial.println("Select value to change or 13  to exit");
    Serial.print("1.  DScalarB       ");
    Serial.println(DScalarB);
    Serial.print("2.  DScalarS       ");
    Serial.println(DScalarS);
    Serial.print("3.  ratioOffset    ");
    Serial.println(ratioOffset);
    Serial.print("4.  shiftSlope     ");
    Serial.println(shiftSlope);
    Serial.print("5.  servoLowlimit  ");
    Serial.println(servoLowlimit);
    Serial.print("6.  servoUpLimit   ");
    Serial.println(servoUpLimit);
    Serial.print("7.  scalarP        ");
    Serial.println(scalarP);
    Serial.print("8.  scalarI        ");
    Serial.println(scalarI);
    Serial.print("9.  powerDiv       ");
    Serial.println(powerDiv);
    Serial.print("10. powerMult      ");
    Serial.println(powerMult);
    Serial.print("11. powerAdd       ");
    Serial.println(powerAdd);
    Serial.print("12. cirWheel       ");
    Serial.println(cirWheel);
    while (Serial.available() ==  0) {}
    BTinPut = Serial.parseInt();


    if (BTinPut == 1) {
      Serial.println();
      Serial.print("DScalarB Enter value ");
      while  (Serial.available() == 0) {}
      DScalarB = (Serial.parseInt());
      EEPROM.put(20,  DScalarB);
      Serial.println();
      Serial.println(" Saved");
    }
    else if (BTinPut == 2) {
      Serial.println();
      Serial.print("DScalarS  Enter value ");
      while (Serial.available() == 0) {}
      DScalarS =  (Serial.parseInt());
      EEPROM.put(21, DScalarS);
      Serial.println();
      Serial.println(" Saved");
    }
    else if (BTinPut == 3) {
      Serial.println();
      Serial.print("ratioOffset Enter value ");
      while (Serial.available()  == 0) {}
      ratioOffset = (Serial.parseInt());
      EEPROM.put(22, ratioOffset);
      Serial.println();
      Serial.println(" Saved");
    }
    else  if (BTinPut == 4) {
      Serial.println();
      Serial.print("shiftSlope  Enter value ");
      while (Serial.available() == 0) {}
      shiftSlope  = (Serial.parseInt());
      EEPROM.put(24, shiftSlope);
      Serial.println();
      Serial.println(" Saved");
    }
    else if (BTinPut == 5) {
      Serial.println();
      Serial.print("servoLowlimit Enter value ");
      while (Serial.available()  == 0) {}
      servoLowlimit = (Serial.parseInt());
      EEPROM.put(26, servoLowlimit);
      Serial.println();
      Serial.println(" Saved");
    }
    else  if (BTinPut == 6) {
      Serial.println();
      Serial.print("servoUpLimit  Enter value ");
      while (Serial.available() == 0) {}
      servoUpLimit  = (Serial.parseInt());
      EEPROM.put(28, servoUpLimit);
      Serial.println();
      Serial.println(" Saved");
    }
    else if (BTinPut == 7) {
      Serial.println();
      Serial.print("scalarP Enter value ");
      while (Serial.available()  == 0) {}
      scalarP = (Serial.parseFloat());
      EEPROM.put(30, scalarP);
      Serial.println();
      Serial.println(" Saved");
    }
    else  if (BTinPut == 8) {
      Serial.println();
      Serial.print("scalarI Enter  value ");
      while (Serial.available() == 0) {}
      scalarI = (Serial.parseFloat());
      EEPROM.put(34, scalarI);
      Serial.println();
      Serial.println("  Saved");
    }
    else if (BTinPut == 9) {
      Serial.println();
      Serial.print("powerDiv Enter value ");
      while (Serial.available()  == 0) {}
      powerDiv = (Serial.parseInt());
      EEPROM.put(38, powerDiv);
      Serial.println();
      Serial.println(" Saved");
    }
    else  if (BTinPut == 10) {
      Serial.println();
      Serial.print("powerMult  Enter value ");
      while (Serial.available() == 0) {}
      powerMult  = (Serial.parseInt());
      EEPROM.put(40, powerMult);
      Serial.println();
      Serial.println(" Saved");
    }
    else if (BTinPut == 11) {
      Serial.println();
      Serial.print("powerAdd Enter value ");
      while  (Serial.available() == 0) {}
      powerAdd = (Serial.parseInt());
      EEPROM.put(41,  powerAdd);
      Serial.println();
      Serial.println(" Saved");
    }
    else if (BTinPut == 12) {
      Serial.println();
      Serial.print("cirWheel  Enter value ");
      while (Serial.available() == 0) {}
      cirWheel =  (Serial.parseInt());
      EEPROM.put(42, cirWheel);
      Serial.println();
      Serial.println(" Saved");
    }
    else if (BTinPut == 13) {
      mode = 3;
      Serial.println("Return to run");
    }
  }
  else if (mode == 8) //servo manual control
  {
    Serial.println("Use  buttons to move servo or enter a value");
    Serial.println("Enter negative  number to exit");

    while (Serial.available() == 0) {
      if (buttonCounter  > 50) {
        servopos ++;
        buttonCounter = 0;
        myservo.write(servopos);
        Serial.println(servopos);
      }

      else if (buttonCounter  < -50) {
        servopos  --;
        buttonCounter = 0;
        myservo.write(servopos);
        Serial.println(servopos);
      }
    }
    servopos = (Serial.parseInt());
    if (servopos >= 0) {
      myservo.write(servopos);
      Serial.println(servopos);
    }
    else {
      mode = 3;
      Serial.println("Return to run");
    }
  }
  else if (mode == 1) //manual
  { /*// alternative manaul

      if (buttonCounter > 80 && servopos  < servoUpLimit){
         servopos  ++;
         buttonCounter = 0;  }

      else if (buttonCounter < -80  && servopos > servoLowlimit){
         servopos  --;
         buttonCounter  = 0;  }

      myservo.write(servopos);
    */
    if (setting == 8)
    {
      servopos = (servoUpLimit);
    }

    else if (setting <=  7 && setting >= -3)
    {
      servopos = ((((servoUpLimit - servoLowlimit)  / 12) * (setting + 4)) + servoLowlimit);
    }

    else if (setting ==  -4)
    {
      servopos = (servoLowlimit);
    }
    myservo.write(servopos);

    //servovolt = analogRead(servopot);                     // read servos internal  pot to get real position
    //servoposError = servovolt - (((servopos - 2) *  2.84) + 83); // calculate error in servos position

    Serial.print("E");
    Serial.print(Power);
    Serial.print(", ");
    Serial.print(Cadence);
    Serial.print(", ");
    Serial.print(Speed);
    Serial.print(", ");
    Serial.print(setting);
    Serial.print(", ");
    Serial.print(servopos);    // (servopos);
    Serial.print(", ");
    Serial.print(lipoPercentP);    //(servoposError);
    Serial.print(", ");
    Serial.println(zeroError);

  }

  else if (Power == 0 || mode == 2) // Constant cadence
  {
    if  (setting == 4)
    {
      cadenceSet = 50;
    }

    else if (setting  == 3)
    {
      cadenceSet = 60;
    }

    else if (setting ==  2)
    {
      cadenceSet = 65;
    }

    else if (setting == 1)
    {
      cadenceSet = 68;
    }

    else if (setting == 0)
    {
      cadenceSet = 72;
    }

    else if (setting == -1)
    {
      cadenceSet = 75;
    }

    else if (setting == -2)
    {
      cadenceSet = 80;
    }

    else if (setting == -3)
    {
      cadenceSet = 90;
    }

    else if (setting == -4)
    {
      cadenceSet = 100;
    }
    multipler = shiftSlope / cadenceSet;

    errorC = (Cadence - prevCadenceSet);

    // calculate offsetP
    if  ((Cadence > (cadenceSet + 10)) && (Cadence < 200))
    {
      offSetP = (multipler  / scalarP);
    }
    else if (Cadence < 38)
    {
      offSetP = 0;
    }
    else {
      offSetP = (sin((errorC) * 0.15)) * (multipler / scalarP);
    }

    // calculate offsetI
    if (Cadence < 38)
    {
      offSetI  = 0;
    }
    else {
      offSetI = constrain ((prevErrorTotal + (errorC  * scalarI)), -1.5, 3); //-1.5, 1.5
    }
    prevErrorTotal = offSetI;
    /*
      // calculate offsetD
      if (Cadence < 38)
         {offSetI  = 0;}
      else {
      offSetD = constrain(((errorC - prevError) * scalarD),-3,15);
                   }
      prevError = errorC;
    */
    offSetC = offSetP  + offSetI; // + offSetD;

    servopos = constrain((ratioOffset + (Wheel *  (multipler + offSetC))), servoLowlimit, servoUpLimit); // calculates servopos, sets  the limits on under/overdrive.
    //multiplier sets the cadence, fixed offset  compensates for difference in ratio profile.
    myservo.write(servopos);

    //CALIBRATE
    /*servovolt = analogRead(servopot);
      servoposError  = servovolt-(((servoposD-2)*2.84)+83);

      Serial.print("E");                //  send values via bluetooth or serial monitor for graphing or logging.
      Serial.print(Power);            // "E" enables an app called Bluetooth Graphics to recognise it as  data.
      Serial.print(", ");
      Serial.print(Cadence);
      Serial.print(",  ");
      Serial.print(Speed);
      Serial.print(", ");
      Serial.print(switchState2);
      Serial.print(", ");
      Serial.print(servoposD);     //cadenceSet
      Serial.print(", ");
      Serial.print(servoposError);   //lipoPercentP
      Serial.print(", ");
      Serial.println(zeroError);
    */

    Serial.print("E");                // send values via bluetooth or serial monitor  for graphing or logging.
    Serial.print(Power);            // "E" enables  an app called Bluetooth Graphics to recognise it as data.
    Serial.print(",  ");
    Serial.print(Cadence);
    Serial.print(", ");
    Serial.print(Speed);
    Serial.print(", ");
    Serial.print(setting);  // lipoPercent
    Serial.print(",  ");
    Serial.print(lipoPercent);    //cadenceSet, servopos, outputTorque
    Serial.print(", ");
    Serial.print(lipoPercentP);
    Serial.print(",  ");
    Serial.println(zeroError);
    //APP
    /*
      Serial.print("  "); //Prevents The operation * cannot accept the arguments: , [*empty-string*],  [-0.24]
      Serial.print(Power);
      Serial.print(",");
      Serial.print(Cadence);
      Serial.print(",");
      Serial.print(Speed);
      Serial.print(",");
      Serial.print(cadenceSet);
      Serial.print(",");
      Serial.print(offSetC);
      Serial.print(",");
      Serial.print(setting);
      Serial.print(",");
      Serial.print(lipoPercent);
      Serial.print(",");
      Serial.print(lipoPercentP);
      Serial.print(",");
      Serial.print(zeroError);
      Serial.print(",");
      Serial.print(inputTorque);
      Serial.print(",");
      Serial.println(outputTorque);  */

    prevCadenceSet = cadenceSet;     //store prev cadence setpoint
    // preServoposD = servoposD;
  }

  else if (Power > 0 && mode == 3)  // Power1 buttons change powerAdd
  {
    // average power 2.5 secs worked  best less results in oscillations, more to much lag
    total = total - readings[index];
    readings[index] = Power;
    total = total + readings[index];
    index  = index + 1;
    if (index >= numReadings)
      index = 0;
    powerAv  = total / numReadings;

    multipler = ((sin(powerAv / powerDiv)) * powerMult)  + (setting + powerAdd);
    cadenceSet = shiftSlope / multipler;

    errorC  = (Cadence - cadenceSet);
    // calculate offsetP
    if ((Cadence > (cadenceSet  + 10)) && (Cadence < 200))
    {
      offSetP = (multipler / 11);
    }
    else if (Cadence < 38)
    {
      offSetP = 0;
    }
    else {
      offSetP = (sin((errorC) * 0.15)) * (multipler / 12);
    }

    //  calculate offsetI
    if (Cadence < 38)
    {
      offSetI = 0;
    }
    else {
      offSetI = constrain ((prevErrorTotal + (errorC * scalarI)),  -1.5, 1.5);
    }
    prevErrorTotal = offSetI;

    offSetC = offSetP  + offSetI; // + offSetD;

    servopos = constrain((ratioOffset + (Wheel *  (multipler + offSetC))), servoLowlimit, servoUpLimit);

    myservo.write(servopos);

    //CALIBRATE
    /*servovolt = analogRead(servopot);
      servoposError  = servovolt-(((servoposD-2)*2.84)+83);

      Serial.print("E");                //  send values via bluetooth or serial monitor for graphing or logging.
      Serial.print(Power);            // "E" enables an app called Bluetooth Graphics to recognise it as  data.
      Serial.print(", ");
      Serial.print(Cadence);
      Serial.print(",  ");
      Serial.print(Speed);
      Serial.print(", ");
      Serial.print(switchState2);
      Serial.print(", ");
      Serial.print(servoposD);     //cadenceSet
      Serial.print(", ");
      Serial.print(servoposError);   //lipoPercentP
      Serial.print(", ");
      Serial.println(zeroError);
    */
    Serial.print("E");
    Serial.print(Power);
    Serial.print(", ");
    Serial.print(Cadence);
    Serial.print(", ");
    Serial.print(Speed);
    Serial.print(", ");
    Serial.print(setting);     // lipoPercent,inputTorque
    Serial.print(",  ");
    Serial.print(lipoPercent);    //cadenceSet, servopos, outputTorque
    Serial.print(", ");
    Serial.print(lipoPercentP);
    Serial.print(",  ");
    Serial.println(zeroError);

    /* //APP
      Serial.print("  "); //Prevents The operation * cannot accept the arguments: , [*empty-string*],  [-0.24]
      Serial.print(Power);
      Serial.print(",");
      Serial.print(Cadence);
      Serial.print(",");
      Serial.print(Speed);
      Serial.print(",");
      Serial.print(cadenceSet);
      Serial.print(",");
      Serial.print(offSetC);
      Serial.print(",");
      Serial.print(setting);
      Serial.print(",");
      Serial.print(lipoPercent);
      Serial.print(",");
      Serial.print(lipoPercentP);
      Serial.print(",");
      Serial.print(zeroError);
      Serial.print(",");
      Serial.print(inputTorque);
      Serial.print(",");
      Serial.println(outputTorque);
    */
  }

  else if (Power > 0 && mode == 4) // Power2 buttons change  powerMult
  {
    // average power
    total = total - readings[index];
    readings[index] = Power;
    total = total + readings[index];
    index  = index + 1;
    if (index >= numReadings)
      index = 0;
    powerAv  = total / numReadings;

    multipler = ((sin(powerAv / powerDiv)) * (setting  + powerMult)) + powerAdd;
    cadenceSet = shiftSlope / multipler;

    errorC  = (Cadence - cadenceSet);
    // calculate offsetP
    if ((Cadence > (cadenceSet  + 10)) && (Cadence < 200))
    {
      offSetP = (multipler / 11);
    }
    else if (Cadence < 38)
    {
      offSetP = 0;
    }
    else {
      offSetP = (sin((errorC) * 0.15)) * (multipler / 12);
    }

    //  calculate offsetI
    if (Cadence < 38)
    {
      offSetI = 0;
    }
    else {
      offSetI = constrain ((prevErrorTotal + (errorC * scalarI)),  -1.5, 1.5);
    }
    prevErrorTotal = offSetI;

    offSetC = offSetP  + offSetI; // + offSetD;

    servopos = constrain((ratioOffset + (Wheel *  (multipler + offSetC))), servoLowlimit, servoUpLimit);

    myservo.write(servopos);

    Serial.print("E");
    Serial.print(Power);
    Serial.print(", ");
    Serial.print(Cadence);
    Serial.print(", ");
    Serial.print(Speed);
    Serial.print(", ");
    Serial.print(setting);     // lipoPercent,inputTorque
    Serial.print(", ");
    Serial.print(lipoPercent);    //cadenceSet, servopos,  outputTorque
    Serial.print(", ");
    Serial.print(lipoPercentP);
    Serial.print(", ");
    Serial.println(zeroError);
    /*
      Serial.print("E");
      Serial.print(Power);
      Serial.print(", ");
      Serial.print(Cadence);
      Serial.print(", ");
      Serial.print(Speed);
      Serial.print(",  ");
      Serial.print(setting);     //lipoPercent
      Serial.print(",  ");
      Serial.print(cadenceSet);     //servopos
      Serial.print(",  ");
      Serial.print(offSetC);   //lipoPercentP
      Serial.print(",  ");
      Serial.println(zeroError);

      //APP
      Serial.print("  "); //Prevents The operation * cannot accept the arguments: , [*empty-string*],  [-0.24]
      Serial.print(Power);
      Serial.print(",");
      Serial.print(Cadence);
      Serial.print(",");
      Serial.print(Speed);
      Serial.print(",");
      Serial.print(cadenceSet);
      Serial.print(",");
      Serial.print(offSetC);
      Serial.print(",");
      Serial.print(setting);
      Serial.print(",");
      Serial.print(lipoPercent);
      Serial.print(",");
      Serial.print(lipoPercentP);
      Serial.print(",");
      Serial.print(zeroError);
      Serial.print(",");
      Serial.print(inputTorque);
      Serial.print(",");
      Serial.println(outputTorque);
    */
  }
}

Downloadable files

Auto Shifter Spreadsheet

Calculate values for code using spreadsheet

Auto Shifter Spreadsheet

Calculate values for code using spreadsheet

Auto Shifter Spreadsheet

Comments

Only logged in users can leave comments

erose

0 Followers

•

0 Projects

Intro