Advertisement
dustinrobotics

ATDservo for Controller 2.0 4-19-2012

Apr 19th, 2012
192
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 17.95 KB | None | 0 0
  1. /* ATDserv
  2. Created by Dustin Reynolds at Mecharobotics.wordpress.com
  3.  
  4. You may share this library with Creative Commons - Share Alike
  5. license 2.0.  
  6.  
  7. This is released via the Creative Commons Share Alike 2.0 license:
  8. http://creativecommons.org/licenses/by-sa/2.0/  
  9.  
  10. You can copy it and use it in whatever, just attribute my name,
  11. Dustin Reynolds, and my blog when you do.  
  12. I'll appreciate if you let me know that you've used it,
  13. so I can show off your project on my blog :)
  14.  
  15. Copyright 2012 Dustin Reynolds
  16. *****************************************************************************
  17.   Includes the following:
  18.   BTM-182 connected to pins 7 and 8
  19.   microSD adapter (using old microSD to SD converter)
  20.  
  21.   Size comparisons:
  22.  
  23. Recommended Flags enabled:
  24.    
  25. Circuit:  Description of the circuit Textually:
  26.   3.3v VReg (MCP1702-3302E)
  27.     1-GND
  28.     2- + of battery, 0.1u to gnd*
  29.     3-3.3v, 0.1u*
  30.     *-1uF across 3.3V and gnd, 0.1 accross 3.3v and gnd
  31.   ATMEGA328
  32.     1-connected to Pin 6 of FTDI cable
  33.     2-Connected to pin 4 of FTDI Cable, Pin 9 of BTM-182 BT serial module(through a 1k resistor), 10k pullup
  34.     3-Connected to pin 5 of FTDI cable, Pin 10 of BTM-182 BT serial module (through a 1k resistor)
  35.             http://www.billporter.info/how-to-add-multiple-uart-connections/
  36.     4-NC
  37.     5-NC
  38.     6-NC
  39.     7-3.3V
  40.     8-gnd
  41.     9-left lead of 16MHz resonator (center to gnd)
  42.     10-right lead of 16MHz resonator(center to gnd)
  43.     11-NC
  44.     12-NC
  45.     13-NC
  46.     14-NC
  47.     15-NC
  48.     16-NC
  49.     17-NC
  50.     18-NC
  51.     19-NC
  52.     20-3.3V
  53.     21-3.3V
  54.     22-GND
  55.     23-NC
  56.     24-NC
  57.     25-NC
  58.     26-NC
  59.     27-NC
  60.     28- 1 lead of NTC thermistor (other lead to gnd), 10K resistor to 3.3v
  61.   BTM-182 BT serial module
  62.      http://www.sparkfun.com/products/9913
  63.      AT commands:http://www.robot-r-us.com/downloads/sparkfun/wigwrl/812-wrl09977-lm400-data-sheet-dss-ver-1-2/download.html
  64.     9-pin 2 of mcu
  65.     10-pin 3 of mcu
  66.     15-3.3v
  67.     16-gnd
  68.   BT using HW serial
  69.   entire system at 3.3v
  70.  
  71. Notes:
  72.   After programming, the FTDI cable will need to be removed, since it interfers with
  73.   BT communication(with the TX/RX lines, not the wireless)
  74.   Dustin Reynolds
  75.   2012-04-19
  76. */
  77.  
  78. #include <Wire.h>
  79. #include <SoftwareSerial.h>
  80. #include <SD.h>
  81. #include <avr/pgmspace.h>
  82. #include <EEPROM.h>
  83. #include <Servo.h>
  84.  
  85. //Comment out these to disable specific features.  Won't disable menus, since that is complicated
  86.  
  87. #define BT_ENABLE
  88. #define BT 1
  89. //#define SD_ENABLE
  90.   #define FILENAME_OFFSET 20 //single byte stored in EEPROM
  91.  
  92. #define THIS_ID     ADC_MONITOR_ID
  93. #define CONTROL_ID  100 //d <-ascii for 100
  94. #define RC_CAR_ID   101  //e
  95. #define ADC_MONITOR_ID   102  //f
  96.  
  97. //types
  98. #define TYPE_UTHERE       0
  99. #define TYPE_DISPTHIS     1
  100. #define TYPE_NUN_CONTROL  2
  101. #define TYPE_PS2_CONTROL  3
  102. #define TYPE_END_COMM     253
  103. #define TYPE_ACK          254
  104. #define TYPE_ERROR        255
  105. #define MAX_PKT_SIZE 30
  106. //options
  107. #define OPTION_SEND_CTL 0x01
  108. #define OPTION_NUN_CTL  0x02
  109. #define OPTION_PS2_CTL  0x04
  110.  
  111. #define TIMEOUT 10
  112. #define SUCCESS 1
  113. #define FAILURE 0
  114.  
  115. File logFile;
  116. #define SDCHIPSEL 10
  117.  
  118. Servo drive;
  119. Servo steer;
  120. //#define DEBUG 1
  121.  
  122. #ifdef BT_ENABLE
  123. //SoftwareSerial btser(7, 8);
  124. #endif
  125.  
  126. const prog_char PROGMEM ppplus[] = "+++";
  127. const prog_char PROGMEM sm1[] = "ATSM1,ST3E8,SO1";
  128. const prog_char PROGMEM wncn[] = "ATWN,CN";
  129. const prog_char PROGMEM atr0[] = "ATR0";
  130. const prog_char PROGMEM ath[] = "ATH";
  131. const prog_char PROGMEM atf[] = "ATF?";
  132. const prog_char PROGMEM atd[] = "ATD";
  133. const prog_char PROGMEM ata[] = "ATA";
  134. const prog_char PROGMEM ato[] = "ATO";
  135. const prog_char PROGMEM Timeout[] = "TimeOut";
  136. const prog_char PROGMEM Exit[] = "Exit";
  137.  
  138. //******************************************************************************
  139. //
  140. // Button Definitions
  141. //
  142. //******************************************************************************
  143. //2-dimensional array for asigning the buttons and there high and low values
  144. const uint16_t PROGMEM Button[21][3]  = {{1, 834, 845}, // button 1
  145.                      {2, 712, 721}, // button 2
  146.                      {3, 603, 613}, // button 3
  147.                      {4, 315, 326}, // button 4
  148.                      {5, 173, 185}, // button 5
  149.                      {6, 85, 97}, // button 6
  150.                      {7, 888, 898}, // button 1 + button 2
  151.                      {8, 872, 882}, // button 1 + button 3
  152.                      {9, 849, 858}, // button 1 + button 4
  153.                      {10, 844, 848}, // button 1 + button 5
  154.                      {11, 838, 843}, // button 1 + button 6
  155.                      {12, 805, 815}, // button 2 + button 3
  156.                      {13, 748, 758}, // button 2 + button 4
  157.                      {14, 729, 740}, // button 2 + button 5
  158.                      {15, 719, 728}, // button 2 + button 6
  159.                      {16, 668, 678}, // button 3 + button 4
  160.                      {17, 636, 646}, // button 3 + button 5
  161.                      {18, 619, 629}, // button 3 + button 6
  162.                      {19, 405, 415}, // button 4 + button 5
  163.                      {20, 359, 369}, // button 4 + button 6
  164.                      {21, 237, 247}}; // button 5 + button 6
  165.  
  166. int setup_ser(byte ser, char * string, char init, char initcr,char justppp, unsigned long timeout, uint16_t analogpin);
  167. uint8_t buttonWait(uint16_t analogpin, uint8_t toggle, uint8_t hold);
  168. uint8_t buttonCheck( uint16_t analogpin);
  169.  
  170. int send_pkt(byte iface, byte dest, byte type, int count, char * buffer)
  171. {
  172.   int checksum;
  173.  
  174.   //X
  175.   Serial.write(0x0a); //LF
  176.   Serial.write(dest);
  177.   //Y
  178.   Serial.write(THIS_ID);//100 is controller
  179.   //Z Type
  180.   Serial.write(type);
  181.   //A count
  182.   Serial.write(count);
  183.   //intialize checksum
  184.   checksum = dest+THIS_ID+type+count;
  185.  
  186.   //get checksum for data
  187.   for(int j=0; j<count; j++)
  188.     checksum +=buffer[j];
  189.  
  190.   //send data
  191.   for(int j=0; j<count; j++)
  192.     Serial.write(buffer[j]);
  193.  
  194.   while (checksum > 256)
  195.     checksum -= 256;
  196.   Serial.write(checksum);
  197.   Serial.write(0x0d); //LF
  198.  
  199.   return 1;
  200. }
  201.  
  202.  
  203. byte get_pkt(byte iface, char* buffer, byte size, uint16_t timeout)
  204. {
  205.   byte temp;
  206.   byte gotchar = 0;
  207.   byte type;
  208.   byte count = 0;
  209.   byte checksum = 0;
  210.   byte theirchecksum = 0;
  211.   byte theirid;
  212.   byte buffoffset = 0;
  213.   byte state = 0;  //0 - wait for LF
  214.                    //1 - next is THIS_ID
  215.                    //2 - their ID
  216.                    //3 - type
  217.                    //4 - count
  218.                    //5-count - packet
  219.                    //last checksum
  220.   unsigned long time1 = millis();
  221.   while(1){
  222.     if(millis() > time1 + timeout)
  223.     {
  224.       //timeout, abort
  225.       memset(buffer, 0, size);
  226.       return TIMEOUT;
  227.     }
  228.    
  229.       gotchar = 0; //only iterate on times in which we received something
  230.                    //don't want to be stuck in 1 big if statement though
  231.       if(Serial.available())
  232.       {
  233.         temp = Serial.read();
  234.         gotchar = 1;
  235.       }
  236.       if(gotchar == 0)
  237.         continue;
  238.  
  239.     //state machine
  240.     if(state == 0)
  241.     {
  242.       if(temp == 0x0A)  //only start processing iff LF received
  243.       {
  244.         state++;
  245.       }
  246.     }
  247.     else if(state == 1)
  248.     {
  249.       if(temp == THIS_ID)
  250.       {
  251.         state++;
  252.         checksum += THIS_ID;
  253.       }
  254.       else
  255.       {
  256.         state = 0;
  257.         count = 0;
  258.         checksum = 0;
  259.         buffoffset = 0;
  260.       }
  261.     }
  262.     else if(state == 2)
  263.     {
  264.       theirid = temp;
  265.       checksum += temp;
  266.       state++;
  267.       *(buffer + buffoffset++) = theirid;
  268.     }
  269.     else if(state == 3)
  270.     {
  271.       type = temp; //save type, not sure what to do with it now
  272.       checksum += type;
  273.       state++;
  274.       *(buffer + buffoffset++) = type;
  275.     }
  276.     else if(state == 4)
  277.     {
  278.       count = temp; //save count
  279.       checksum += count;
  280.       if(count == 0) //empty packet?
  281.         state++; //skip packet step, go straight to checksum
  282.       state++;
  283.       *(buffer + buffoffset++) = count;
  284.     }
  285.     else if(state == 5)
  286.     {
  287.       //stay in this state until count bytes arrive, or until timeout
  288.       //count guaranteed to be > 0 initially
  289.       *(buffer + buffoffset++) = temp;  //save in buffer
  290.       checksum += temp;
  291.       if(--count == 0)
  292.       {
  293.         state++;
  294.       }
  295.     }
  296.     else if(state == 6)
  297.     {
  298.       //checksum
  299.       theirchecksum = temp;
  300.      
  301.       if(theirchecksum == checksum)
  302.       {
  303.         //Buffer valid, return to calling program
  304.         return SUCCESS; //data stored in provided buffer
  305.       }
  306.       else
  307.       {
  308.         //checksum not valid, return to state 0
  309.         state = 0;
  310.         memset(buffer, 0, size);
  311.       }
  312.     }
  313.   }
  314. }
  315. //buffer must be at least 5 big
  316. //
  317. // offset starts at 5, goes down as the number is larger
  318. // so that it is right aligned
  319. byte convert_string(char * string, short n)
  320. {
  321.   byte count = 0;
  322.   if(n>=10)
  323.   {
  324.     count = convert_string(string, n/10);
  325.     n = n%10;
  326.   }
  327.   *(string + count) = n+'0';
  328.   return count+1;
  329. }
  330.  
  331.  
  332. /* Handle packet - Does everything conceivable for a received packet
  333.     -handles control data from controller
  334.     -sends updated ATD measurements
  335.  
  336. */
  337. byte handle_pkt(byte iface, byte dest, byte options, uint16_t sendctl, uint16_t timeout)
  338. {
  339.   unsigned long time_timeout = millis() + timeout;
  340.   unsigned long time_sendctl = millis() + sendctl;
  341.   char buffer[MAX_PKT_SIZE];
  342.   char outbuffer[MAX_PKT_SIZE];
  343.   byte thisdest;
  344.   byte type;
  345.   byte nthings;
  346.   byte col;
  347.   byte row;
  348.   byte val;
  349.   byte error;
  350.   byte zbutton, cbutton, joy_x,joy_y;
  351.   int temp;
  352.   int tempx,tempy,tempz;
  353.   int i = 0;
  354.  
  355.   while(1)
  356.   {
  357.     //update device, if we are using it
  358.  
  359.     if(millis() > time_timeout)
  360.     {
  361.       //break;
  362.     }
  363.     if(millis() > time_sendctl)
  364.     {
  365.       if(options & OPTION_SEND_CTL)
  366.       {
  367.         memset(buffer,0,MAX_PKT_SIZE);
  368.         i = 0;
  369.        
  370.         //get packet ready to send
  371.        
  372.         //clear display
  373.         send_pkt(BT, CONTROL_ID, TYPE_DISPTHIS,0,buffer);
  374.        
  375.         type = TYPE_DISPTHIS;
  376.        
  377.         buffer[i++] = 0; //row;
  378.         buffer[i++] = 0; //col
  379.        
  380.         strncpy(buffer+i,"volt:", 20);  //max of 1 row at a time, received string
  381.         i=7;
  382.         temp = analogRead(A5);
  383.        
  384.         i+=convert_string(&buffer[i], temp);
  385.         send_pkt(BT, CONTROL_ID, TYPE_DISPTHIS,i,buffer);
  386.       }
  387.       time_sendctl = millis() + sendctl;
  388.     }
  389.  
  390.     error = get_pkt(iface, buffer, MAX_PKT_SIZE, 1000); //longer delay since not actively controlling something
  391.    
  392.     //process packet
  393.     //if(error == TIMEOUT)
  394.     //  send_pkt(iface,dest,TYPE_ERROR,0,buffer);
  395.     if(error == SUCCESS)
  396.     {
  397.       i = 0;
  398.       //their id
  399.       thisdest = buffer[i++];
  400.       if(thisdest!= dest)
  401.       {
  402.         //should we process this?  leave it for now
  403.       }
  404.       else
  405.       {
  406.         //reset time, since we recently received something.
  407.         time_timeout = millis() + timeout;
  408.       }
  409.       //read type
  410.       type = buffer[i++];
  411.      
  412.       memset(outbuffer,0,sizeof(outbuffer));
  413.       switch(type)
  414.       {
  415.         case TYPE_UTHERE:
  416.         {
  417.           nthings = buffer[i++];
  418.          
  419.           outbuffer[0] = 'Y';
  420.           if(buffer[i] == 'P')
  421.             send_pkt(iface,thisdest,TYPE_UTHERE,1,outbuffer);
  422.            
  423.           //else, do nothing, no response needed
  424.           break;
  425.         }
  426.         case TYPE_DISPTHIS:
  427.         {
  428.           //disp can be on controller, or remote thing!
  429.  
  430.           break;
  431.         }
  432.         case TYPE_NUN_CONTROL:
  433.         {
  434.           //use joy_x and joy_y to control servos
  435.           //data is in buffer
  436.           nthings = buffer[i++];
  437.          
  438.           zbutton = buffer[i++];
  439.           cbutton = buffer[i++];
  440.           joy_x = buffer[i++];
  441.           joy_y = buffer[i++];
  442.          
  443.           tempx = buffer[i++] << 8;
  444.           tempx += buffer[i++];
  445.           tempy = buffer[i++] << 8;
  446.           tempy += buffer[i++];
  447.           tempz = buffer[i++] << 8;
  448.           tempz += buffer[i++];
  449.          
  450.           //now have everything, turn servos now
  451.           joy_x = map(joy_x, 0, 255, 0, 179);
  452.           joy_y = map(joy_y, 0, 255, 0, 179);
  453.          
  454.           drive.write(joy_x);
  455.           steer.write(joy_y);
  456.  
  457.           break;
  458.         }
  459.         case TYPE_PS2_CONTROL:
  460.         {
  461.           //for controller, what does this mean?
  462.  
  463.           break;
  464.         }
  465.         case TYPE_ACK:
  466.         {
  467.           //last packet received OK
  468.           break;
  469.         }
  470.         case TYPE_ERROR:
  471.         {
  472.           //last packet not received, resend depending on mode perhaps?
  473.           break;
  474.         }
  475.         case TYPE_END_COMM:
  476.         {
  477.           //remote requests end of comm
  478.           send_pkt(iface,thisdest,TYPE_END_COMM,0,outbuffer);//send packet acking this
  479.           break;
  480.         }
  481.         default:
  482.         {
  483.           //future type?
  484.           break;
  485.         }
  486.       }
  487.      
  488.     }
  489.    
  490.     //exit condition?
  491.     if(analogRead(A0) > 70)
  492.     {
  493.       while(analogRead(A0) > 70){};
  494.       break;
  495.     }
  496.   }
  497. }
  498.  
  499. void setup()
  500. {
  501. #ifdef BT_ENABLE
  502.   Serial.begin(9600);
  503. #endif
  504. #ifdef SD_ENABLE
  505.   pinMode(10, OUTPUT);
  506. #endif
  507.  
  508.   drive.attach(2);
  509.   steer.attach(3);
  510. }
  511.  
  512. void loop()
  513. {
  514.   byte options = 0;
  515.   options = OPTION_SEND_CTL | OPTION_NUN_CTL;
  516.   handle_pkt(BT, CONTROL_ID, options, 100, 20000);
  517.  
  518. }
  519.  
  520. int setup_ser(byte ser, PGM_P PROGMEM string, char init, char initcr, char justppp, unsigned long timeout,uint16_t analogpin)
  521. {
  522.   char state=0, temp;
  523.   //unsigned long time1 = millis();
  524.  
  525.   if(init){
  526.     Serial.print((__FlashStringHelper *)ppplus);
  527.     if(initcr)
  528.       Serial.write(0x0D);
  529.   }
  530.   else
  531.     state = 3;
  532.   while(1)
  533.   {
  534.     if(state == 3)
  535.     {
  536.       state++;
  537.       if(justppp)
  538.         continue;
  539.       Serial.print((__FlashStringHelper *)string);
  540.       Serial.write(0x0D);
  541.       delay(100);
  542.     }
  543.     if(Serial.available())
  544.     {
  545.       temp = Serial.read();
  546.       if(temp == 0x0D)
  547.       {
  548.         state++;
  549.         continue;
  550.       }
  551.      
  552.       if(temp == 'O')
  553.         state++;
  554.       if(temp == 'K')
  555.         state++;
  556.  
  557.       //if(temp == 'E' || temp == 'R')
  558.       //  return 0; //error
  559.     }
  560.     if(analogRead(analogpin) > 70)
  561.     {
  562.       while(analogRead(analogpin) > 70){};
  563.       return 0;
  564.     }
  565.   }
  566.  
  567.  return 1;
  568. }
  569.  
  570. //reserved for future
  571. #if 0
  572. int scan_bt_menuize(byte timeout)
  573. {
  574.   char state=0, temp, skip, devicenum;
  575.   char sindex[20];
  576.   char buffer[10][20];
  577.   char mac[10][15];
  578.   unsigned long time1 = millis();
  579.   lcd.clear();
  580.  
  581.   //send ATF?
  582.   Serial.print((__FlashStringHelper *)atf); //ATF?
  583.   Serial.write(0x0D);
  584.   //create own menu system here, pretty much.  Need to be able to select, and connect to it
  585.   while(skip < 25) //17
  586.   {
  587.     if(Serial.available())
  588.     {
  589.       temp = Serial.read();
  590.       //lcd.print((char)temp);
  591.       //delay(250);
  592.       skip++;
  593.     }
  594.     //if(millis() > time1 + timeout)
  595.     //{
  596.     //  lcd.print((__FlashStringHelper *)Timeout);
  597.     //  delay(1000);
  598.     //  break;
  599.     //}
  600.     if(analogRead(A0) > 70)
  601.       return 0;
  602.   }
  603.  
  604.  
  605.   //process each result from BT module, displaying it as you go
  606.   while(1)
  607.   {
  608.     temp = 0;
  609.     int i = 0;  //cnt of number of BT things around
  610.     skip = 0;
  611.     while(skip < 3)  //skip LF and first number.  Land in space region before name
  612.     {
  613.       if(Serial.available())
  614.       {
  615.         temp = Serial.read();
  616.         //lcd.print((char)temp);
  617.         skip++;
  618.       }
  619.       if(analogRead(A0) > 70)
  620.         return 0;
  621.     }
  622.     //now read spaces until nonspace
  623.     while(temp == ' ')
  624.     {
  625.       if(Serial.available())
  626.       {
  627.         temp = Serial.read();
  628.         //lcd.print((char)temp);
  629.       }
  630.       if(analogRead(A0) > 70)
  631.         return 0;
  632.     }
  633.     //lcd.clear();
  634.     //lcd.print("proc Name");lcd.print((int)devicenum);
  635.     //lcd.setCursor(0,1);
  636.     //Now process this name
  637.     buffer[devicenum][i++] = temp;
  638.     //lcd.print((char)temp);
  639.     while(temp != ' ')
  640.     {
  641.       if(Serial.available())
  642.       {
  643.         temp = Serial.read();
  644.         //lcd.print((char)temp);
  645.         //delay(250);
  646.         if(temp != ' ')
  647.           buffer[devicenum][i++]=temp; //save name
  648.       }
  649.       //if(analogRead(A0) > 70)
  650.       //  return 0;
  651.       if(analogRead(A0) > 70)
  652.       {
  653.         lcd.print((int)i);
  654.         while(analogRead(A0) > 70){};
  655.         //return 0;
  656.       }
  657.     }
  658.     buffer[devicenum][i] = 0; //null terminated
  659.     //delay(2000);
  660.    
  661.     //lcd.clear();
  662.     //lcd.print("remove space");
  663.     //lcd.setCursor(0,1);
  664.     //now read spaces until nonspace
  665.     while(temp == ' ')
  666.     {
  667.       if(Serial.available())
  668.       {
  669.         temp = Serial.read();
  670.         //lcd.print((char)temp);
  671.       }
  672.       if(analogRead(A0) > 70)
  673.         return 0;
  674.     }
  675.     i=0;
  676.     //delay(2000);
  677.     //lcd.clear();
  678.     //lcd.print("proc Mac");
  679.     //lcd.setCursor(0,1);
  680.     //now BT mac, need to save this too!
  681.     mac[devicenum][i++]=temp;  //temp is first nonzero from above
  682.     while(i < 14)  //expect XXXX-XX-XXXXXX
  683.     {
  684.       if(Serial.available())
  685.       {
  686.         temp = Serial.read();
  687.         //lcd.print((char)temp);
  688.         //delay(250);
  689.         if(temp != 0x0A)  //should never be 0x0A, if it is, we missed a character
  690.           mac[devicenum][i++]=temp; //save name
  691.       }
  692.       if(analogRead(A0) > 70)
  693.       {
  694.         lcd.print((__FlashStringHelper *)Exit);
  695.         while(analogRead(A0) > 70){};
  696.         return 0;
  697.       }
  698.     }
  699.     mac[devicenum][i] = 0;
  700.    
  701.    
  702.     //lcd.clear();
  703.     //now we have device name, and mac, both null terminated.  Display device name
  704.     lcd.setCursor(1,devicenum); //leave space for cursor
  705.     lcd.print(buffer[devicenum++]);  //hopefully it is less than 19 chars long
  706.     delay(1000);
  707.     time1=millis();
  708.     while(millis() < time1 + 4000)
  709.     {
  710.       if(Serial.available())
  711.       {
  712.         temp = Serial.read();
  713.         //lcd.print((char)temp);
  714.         //delay(250);
  715.         break;
  716.       }
  717.     }
  718.     if(millis() < time1 + 4000)
  719.       return 0;
  720.   }
  721.  
  722. }
  723. #endif
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement